XXL.h
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 #ifndef OMPL_GEOMETRIC_PLANNERS_XXL_XXL_
38 #define OMPL_GEOMETRIC_PLANNERS_XXL_XXL_
39 
40 #include <thread>
41 #include <unordered_map>
42 #include "ompl/util/Hash.h"
43 #include "ompl/datastructures/AdjacencyList.h"
44 #include "ompl/geometric/planners/PlannerIncludes.h"
45 #include "ompl/geometric/planners/xxl/XXLDecomposition.h"
46 
47 
48 namespace ompl
49 {
50  namespace geometric
51  {
64  class XXL : public base::Planner
65  {
66  public:
67  // Standard planner constructor. Must call setDecomposition before calling solve()
68  XXL(const base::SpaceInformationPtr &si);
69 
70  // Initialize HiLo with the given decomposition
71  XXL(const base::SpaceInformationPtr &si, const XXLDecompositionPtr &decomp);
72 
73  virtual ~XXL();
74 
75  virtual void getPlannerData(base::PlannerData &data) const;
76 
77  virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
78 
79  virtual void clear();
80 
81  virtual void setup();
82 
83  void setDecomposition(const XXLDecompositionPtr &decomp);
84 
85  double getRandWalkRate() const
86  {
87  return rand_walk_rate_;
88  }
89  void setRandWalkRate(double rate)
90  {
91  if (rate < 0.0 || rate > 1.0)
92  throw;
93  rand_walk_rate_ = rate;
94  }
95 
96  protected:
97  // Quickly insert, check membership, and grab a unique integer from a range [0, max)
98  class PerfectSet
99  {
100  public:
101  PerfectSet(std::size_t max)
102  {
103  exists.assign(max, false);
104  elements.reserve(max / 10); // reserve, but do not "allocate" the space
105  }
106 
107  std::size_t numElements() const
108  {
109  return elements.size();
110  }
111 
112  bool isElement(unsigned int val) const
113  {
114  if (val >= (unsigned int)exists.size())
115  return false;
116  return exists[val];
117  }
118 
119  bool addElement(unsigned int val)
120  {
121  if (val >= (unsigned int)exists.size() || exists[val])
122  return false;
123 
124  elements.push_back(val);
125  exists[val] = true;
126  return true;
127  }
128 
129  int getElement(std::size_t idx) const
130  {
131  return elements[idx];
132  }
133 
134  protected:
135  std::vector<bool> exists;
136  std::vector<unsigned int> elements;
137  };
138 
139  struct Motion
140  {
141  base::State *state;
142  std::vector<int> levels;
143  int index;
144  };
145 
146  struct Region
147  {
148  std::vector<int> allMotions;
149  std::vector<int> motionsInTree; // subset of allMotions that are connected to the tree
150  };
151 
152  class Layer
153  {
154  public:
155  Layer(int _id, int numRegions, int lvl, Layer *_parent)
156  : regions(numRegions)
157  , weights(numRegions, 0.5)
158  , exterior(numRegions, true)
159  , connections(numRegions, 0)
160  , selections(numRegions, 0)
161  , leads(numRegions, 0)
162  , goalStates(numRegions, std::vector<int>())
163  , connectionPoints(numRegions)
164  , regionGraph(new AdjacencyList(numRegions))
165  , level(lvl)
166  , id(_id)
167  , parent(_parent)
168  {
169  }
170 
171  ~Layer()
172  {
173  delete regionGraph;
174  for (auto &sublayer : sublayers)
175  delete sublayer;
176  }
177 
178  size_t numRegions() const
179  {
180  return regions.size();
181  }
182 
183  int getLevel() const
184  {
185  return level;
186  }
187 
188  Layer *getParent() const
189  {
190  return parent;
191  }
192 
193  int getID() const
194  {
195  return id;
196  }
197 
198  Region &getRegion(int r)
199  {
200  if (r < 0 || r >= (int)regions.size())
201  {
202  OMPL_ERROR("Requested region %d, but there are only %lu regions", r, regions.size());
203  throw ompl::Exception("Region out of bounds");
204  }
205 
206  return regions[r];
207  }
208 
209  const Region &getRegion(int r) const
210  {
211  if (r < 0 || r >= (int)regions.size())
212  {
213  OMPL_ERROR("Requested region %d, but there are only %lu regions", r, regions.size());
214  throw ompl::Exception("Region out of bounds");
215  }
216 
217  return regions[r];
218  }
219 
220  const std::vector<double> &getWeights() const
221  {
222  return weights;
223  }
224 
225  std::vector<double> &getWeights()
226  {
227  return weights;
228  }
229 
230  const std::vector<bool> &getExterior() const
231  {
232  return exterior;
233  }
234 
235  std::vector<bool> &getExterior()
236  {
237  return exterior;
238  }
239 
240  const std::vector<int> &getConnections() const
241  {
242  return connections;
243  }
244 
245  const std::vector<int> &getSelections() const
246  {
247  return selections;
248  }
249 
250  const std::vector<std::vector<int>> &getGoalStates() const
251  {
252  return goalStates;
253  }
254 
255  const std::vector<int> &getGoalStates(int reg) const
256  {
257  return goalStates[reg];
258  }
259 
260  size_t numGoalStates() const
261  {
262  return totalGoalStates;
263  }
264 
265  size_t numGoalStates(int reg) const
266  {
267  return goalStates[reg].size();
268  }
269 
270  void addGoalState(int reg, int id)
271  {
272  goalStates[reg].push_back(id);
273  totalGoalStates++;
274  }
275 
276  AdjacencyList &getRegionGraph()
277  {
278  return *regionGraph;
279  }
280 
281  const AdjacencyList &getRegionGraph() const
282  {
283  return *regionGraph;
284  }
285 
286  Layer *getSublayer(int l)
287  {
288  return sublayers[l];
289  }
290 
291  const Layer *getSublayer(int l) const
292  {
293  return sublayers[l];
294  }
295 
296  void allocateSublayers()
297  {
298  if (sublayers.size())
299  throw;
300 
301  for (size_t i = 0; i < regions.size(); ++i)
302  sublayers.push_back(new Layer(i, regions.size(), level + 1, this));
303  }
304 
305  bool hasSublayers()
306  {
307  return !sublayers.empty();
308  }
309 
310  void selectRegion(int r, int count = 1)
311  {
312  // numSelections++;
313  // selections[r]++;
314  numSelections += count;
315  selections[r] += count;
316  }
317 
318  void connectRegion(int r)
319  {
320  numConnections++;
321  connections[r]++;
322  connectionPoints.addElement(r);
323  }
324 
325  int totalSelections() const
326  {
327  return numSelections;
328  }
329 
330  int totalConnections() const
331  {
332  return numConnections;
333  }
334 
335  int connectibleRegions() const
336  {
337  return connectionPoints.numElements();
338  }
339 
340  int connectibleRegion(int idx) const
341  {
342  return connectionPoints.getElement(idx);
343  }
344 
345  int leadAppearances(int idx) const
346  {
347  return leads[idx];
348  }
349 
350  int numLeads() const
351  {
352  return numTotalLeads;
353  }
354 
355  void markLead(const std::vector<int> &lead)
356  {
357  numTotalLeads++;
358  for (size_t i = 0; i < lead.size(); ++i)
359  leads[lead[i]]++;
360  }
361 
362  protected:
363  std::vector<Region> regions; // The list of regions in this layer
364  std::vector<double> weights; // Weight for each region
365  std::vector<bool> exterior; // Exterior status for the regions in this layer
366  std::vector<int> connections; // Number of times the search has tried internal connections in this
367  // region
368  std::vector<int> selections; // Number of times the search has selected this region for expansion
369  std::vector<int> leads; // Number of times each region has appeared in a lead
370  std::vector<std::vector<int>> goalStates; // A list of goal states in each region
371  PerfectSet connectionPoints; // The set of regions we have tried to do internal connections on
372 
373  AdjacencyList *regionGraph; // The connectivity of regions in this layer
374  std::vector<Layer *> sublayers; // The layers descending from this layer
375  int level; // The level of this layer in the hierarchy (0 is top)
376  int numSelections{0}; // The total number of selections in this layer
377  int numConnections{0}; // The total number of internal connection attempts in this layer
378  int id;
379  int totalGoalStates{0}; // The total number of goal states in this layer
380  int numTotalLeads{0};
381  Layer *parent;
382  };
383 
384  void freeMemory();
385  void allocateLayers(Layer *layer);
386 
387  void updateRegionConnectivity(const Motion *m1, const Motion *m2, int layer);
388  Layer *getLayer(const std::vector<int> &regions, int layer);
389 
390  int addState(const base::State *state);
391  int addThisState(base::State *state); // add state using this state memory, no copy
392  int addGoalState(const base::State *state);
393  int addStartState(const base::State *state);
394 
395  // Update the various statistics for the regions and their subregions in the vector
396  void updateRegionProperties(const std::vector<int> &regions);
397  // Update the various statistics for the region specified
398  void updateRegionProperties(Layer *layer, int region);
399 
400  // Sample states uniformly at random in the given layer until ptc is triggered
401  void sampleStates(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc);
402  bool sampleAlongLead(Layer *layer, const std::vector<int> &lead,
404 
405  int steerToRegion(Layer *layer, int from, int to);
406  int expandToRegion(Layer *layer, int from, int to, bool useExisting = false);
407 
408  bool feasibleLead(Layer *layer, const std::vector<int> &lead,
410  bool connectLead(Layer *layer, const std::vector<int> &lead, std::vector<int> &candidateRegions,
412  void connectRegion(Layer *layer, int region, const base::PlannerTerminationCondition &ptc);
413  void connectRegions(Layer *layer, int r1, int r2, const base::PlannerTerminationCondition &ptc,
414  bool all = false);
415 
416  // Compute a new lead in the given decomposition layer from start to goal
417  void computeLead(Layer *layer, std::vector<int> &lead);
418 
419  // Search for a solution path in the given layer
420  bool searchForPath(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc);
421 
422  // Return a list of neighbors and the edge weights from rid
423  void getNeighbors(int rid, const std::vector<double> &weights,
424  std::vector<std::pair<int, double>> &neighbors) const;
425 
426  // Shortest (weight) path from r1 to r2
427  bool shortestPath(int r1, int r2, std::vector<int> &path, const std::vector<double> &weights);
428 
429  // Compute a path from r1 to r2 via a random walk
430  bool randomWalk(int r1, int r2, std::vector<int> &path);
431 
432  void getGoalStates();
433  // Thread that gets us goal states
434  void getGoalStates(const base::PlannerTerminationCondition &ptc);
435 
436  bool constructSolutionPath();
437 
438  bool isStartState(int idx) const;
439  bool isGoalState(int idx) const;
440 
441  void writeDebugOutput() const;
442 
443  // Root layer of the decomposition data
444  Layer *topLayer_{nullptr};
445 
446  // Raw storage for all motions explored during search
447  std::vector<Motion *> motions_;
448  // Indexes into motions_ for start states
449  std::vector<int> startMotions_;
450  // Index into motions_ for goal states
451  std::vector<int> goalMotions_;
452  // The number of goal states in each decomposition cell
453  std::unordered_map<std::vector<int>, int> goalCount_;
454 
455  base::State *xstate_;
456 
457  // The number of states in realGraph that have verified edges in the graph
458  unsigned int statesConnectedInRealGraph_;
459 
460  unsigned int maxGoalStatesPerRegion_;
461  unsigned int maxGoalStates_;
462 
463  // Random number generator
464  RNG rng_;
465 
466  base::StateSamplerPtr sampler_;
467 
468  // A decomposition of the search space
469  XXLDecompositionPtr decomposition_;
470 
471  // A lazily constructed graph where edges between states have not been collision checked
472  AdjacencyList lazyGraph_;
473  // The verified graph where all edges have been collision checked. An edge in this graph
474  // should not exist in lazyGraph
475  AdjacencyList realGraph_;
476 
477  // Variable for the goal state sampling thread
478  bool kill_{false};
479 
480  // Scratch space for shortest path computation
481  std::vector<int> predecessors_;
482  std::vector<bool> closedList_;
483 
484  double rand_walk_rate_{-1.0};
485  };
486  } // namespace geometric
487 } // namespace ompl
488 
489 #endif
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 void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: XXL.cpp:109
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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
bool searchForPath(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc)
Definition: XXL.cpp:1288
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
The exception type for ompl.
Definition: Exception.h:78
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A shared pointer wrapper for ompl::geometric::XXLDecomposition.