Loading...
Searching...
No Matches
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...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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()