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
48namespace 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
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)
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
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
The exception type for ompl.
Definition: Exception.h:47
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
Definition of an abstract state.
Definition: State.h:50
A shared pointer wrapper for ompl::geometric::XXLDecomposition.
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: XXL.cpp:109
bool searchForPath(Layer *layer, const ompl::base::PlannerTerminationCondition &ptc)
Definition: XXL.cpp:1288
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: XXL.cpp:1464
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: XXL.cpp:65
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: XXL.cpp:1339
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49