Loading...
Searching...
No Matches
BITstar.h
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, University of Toronto
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 University of Toronto 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/* Authors: Jonathan Gammell, Marlin Strub */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_
38#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_
39
40#include <string>
41#include <utility>
42#include <vector>
43
44#include "ompl/base/Planner.h"
45#include "ompl/base/samplers/InformedStateSampler.h"
46#include "ompl/datastructures/NearestNeighbors.h"
47
48// Defining BITSTAR_DEBUG enables (significant) debug output. Do not enable unless necessary.
49// #define BITSTAR_DEBUG
50
51namespace ompl
52{
53 namespace geometric
54 {
93 {
94 public:
95 // ---
96 // Forward declarations.
97 // ---
98
100 class Vertex;
102 class IdGenerator;
104 class CostHelper;
106 class ImplicitGraph;
109 class SearchQueue;
110
111 // ---
112 // Aliases.
113 // ---
114
116 using VertexPtr = std::shared_ptr<Vertex>;
117
119 using VertexConstPtr = std::shared_ptr<const Vertex>;
120
122 using VertexWeakPtr = std::weak_ptr<Vertex>;
123
125 using VertexPtrVector = std::vector<VertexPtr>;
126
128 using VertexConstPtrVector = std::vector<VertexConstPtr>;
129
131 using VertexId = unsigned int;
132
134 using VertexPtrPair = std::pair<VertexPtr, VertexPtr>;
135
137 using VertexConstPtrPair = std::pair<VertexConstPtr, VertexConstPtr>;
138
140 using VertexPtrPairVector = std::vector<VertexPtrPair>;
141
143 using VertexConstPtrPairVector = std::vector<VertexConstPtrPair>;
144
146 using VertexPtrNNPtr = std::shared_ptr<NearestNeighbors<VertexPtr>>;
147
149 using NameFunc = std::function<std::string()>;
150
152 BITstar(const base::SpaceInformationPtr &spaceInfo, const std::string &name = "BITstar");
153
155 virtual ~BITstar() override = default;
156
158 void setup() override;
159
161 void clear() override;
162
164 base::PlannerStatus solve(const base::PlannerTerminationCondition &terminationCondition) override;
165
167 void getPlannerData(base::PlannerData &data) const override;
168
169 // ---
170 // Debugging info.
171 // ---
172
175 std::pair<const ompl::base::State *, const ompl::base::State *> getNextEdgeInQueue();
176
181
183 void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue);
184
186 unsigned int numIterations() const;
187
190
192 unsigned int numBatches() const;
193
194 // ---
195 // Settings.
196 // ---
197
199 void setRewireFactor(double rewireFactor);
200
202 double getRewireFactor() const;
203
206
209
211 void setSamplesPerBatch(unsigned int n);
212
214 unsigned int getSamplesPerBatch() const;
215
217 void setUseKNearest(bool useKNearest);
218
220 bool getUseKNearest() const;
221
225 void setStrictQueueOrdering(bool beStrict);
226
228 bool getStrictQueueOrdering() const;
229
234 void setPruning(bool prune);
235
237 bool getPruning() const;
238
241 void setPruneThresholdFraction(double fractionalChange);
242
245 double getPruneThresholdFraction() const;
246
252 void setDelayRewiringUntilInitialSolution(bool delayRewiring);
253
256
266 void setJustInTimeSampling(bool useJit);
267
269 bool getJustInTimeSampling() const;
270
277 void setDropSamplesOnPrune(bool dropSamples);
278
280 bool getDropSamplesOnPrune() const;
281
284 void setStopOnSolnImprovement(bool stopOnChange);
285
287 bool getStopOnSolnImprovement() const;
288
290 void setConsiderApproximateSolutions(bool findApproximate);
291
294
296 template <template <typename T> class NN>
297 void setNearestNeighbors();
298
299 protected:
300 // ---
301 // The settings that turn BIT* into ABIT*.
302 // ---
303
306 void setInitialInflationFactor(double factor);
307
310 void setInflationScalingParameter(double parameter);
311
314 void setTruncationScalingParameter(double parameter);
315
317 void enableCascadingRewirings(bool enable);
318
319 // ---
320 // Getters specific to ABIT*.
321 // ---
322
324 double getInitialInflationFactor() const;
325
327 double getInflationScalingParameter() const;
328
330 double getTruncationScalingParameter() const;
331
333 double getCurrentInflationFactor() const;
334
336 double getCurrentTruncationFactor() const;
337
338 private:
339 // ---
340 // High level primitives.
341 // ---
342
344 void iterate();
345
347 void newBatch();
348
350 void prune();
351
353 void publishSolution();
354
355 // ---
356 // Low level primitives.
357 // ---
358
361 std::vector<const ompl::base::State *> bestPathFromGoalToStart() const;
362
365 bool checkEdge(const VertexConstPtrPair &edge);
366
368 void blacklistEdge(const VertexPtrPair &edge) const;
369
371 void whitelistEdge(const VertexPtrPair &edge) const;
372
375 void addEdge(const VertexPtrPair &edge, const ompl::base::Cost &edgeCost);
376
378 void replaceParent(const VertexPtrPair &edge, const ompl::base::Cost &edgeCost);
379
381 void updateGoalVertex();
382
383 // ---
384 // Logging.
385 // ---
386
387 // Helper functions for logging
389 void goalMessage() const;
390
392 void endSuccessMessage() const;
393
395 void endFailureMessage() const;
396
398 void statusMessage(const ompl::msg::LogLevel &logLevel, const std::string &status) const;
399
400 // ---
401 // Progress properties.
402 // ---
403
405 std::string bestCostProgressProperty() const;
406
409 std::string bestLengthProgressProperty() const;
410
413 std::string currentFreeProgressProperty() const;
414
417 std::string currentVertexProgressProperty() const;
418
421 std::string edgeQueueSizeProgressProperty() const;
422
424 std::string iterationProgressProperty() const;
425
427 std::string batchesProgressProperty() const;
428
430 std::string pruningProgressProperty() const;
431
434 std::string totalStatesCreatedProgressProperty() const;
435
438 std::string verticesConstructedProgressProperty() const;
439
442 std::string statesPrunedProgressProperty() const;
443
446 std::string verticesDisconnectedProgressProperty() const;
447
450 std::string rewiringProgressProperty() const;
451
454 std::string stateCollisionCheckProgressProperty() const;
455
458 std::string edgeCollisionCheckProgressProperty() const;
459
462 std::string nearestNeighbourProgressProperty() const;
463
466 std::string edgesProcessedProgressProperty() const;
467
468 // ---
469 // Member variables (Make all are configured in setup() and reset in reset()).
470 // ---
471
473 std::shared_ptr<CostHelper> costHelpPtr_{nullptr};
474
476 std::shared_ptr<ImplicitGraph> graphPtr_{nullptr};
477
481 std::shared_ptr<SearchQueue> queuePtr_{nullptr};
482
484 double initialInflationFactor_{1.0};
485
487 double truncationFactor_{1.0};
488
490 double truncationScalingParameter_{0.0};
491
493 double inflationScalingParameter_{0.0};
494
496 VertexConstPtr curGoalVertex_{nullptr};
497
500 // Gets set in setup to the proper calls from OptimizationObjective
501 ompl::base::Cost bestCost_;
502
505 unsigned int bestLength_{0u};
506
509 // Gets set in setup to the proper calls from OptimizationObjective.
510 ompl::base::Cost prunedCost_{std::numeric_limits<double>::infinity()};
511
514 // Gets set in setup with the proper call to Planner::si_->getSpaceMeasure()
515 double prunedMeasure_{0.0};
516
517 bool isFinalSearchOnBatch_{false};
518
520 bool hasExactSolution_{false};
521
523 bool isSearchDone_{false};
524
526 bool stopLoop_{false};
527
529 unsigned int numBatches_{0u};
530
533 unsigned int numPrunings_{0u};
534
536 unsigned int numIterations_{0u};
537
539 unsigned int numRewirings_{0u};
540
542 unsigned int numEdgeCollisionChecks_{0u};
543
544 // ---
545 // Parameters - Set defaults in construction/setup and do not reset in clear.
546 // ---
547
549 unsigned int samplesPerBatch_{100u};
550
552 bool isPruningEnabled_{true};
553
555 double pruneFraction_{0.05};
556
558 bool stopOnSolutionChange_{false};
559 }; // class BITstar
560 } // namespace geometric
561} // namespace ompl
562
563#endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
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
A helper class to handle the various heuristic functions in one place.
Definition: CostHelper.h:70
An ID generator class for vertex IDs.
Definition: IdGenerator.h:59
A conceptual representation of samples as an edge-implicit random geometric graph.
Definition: ImplicitGraph.h:57
A queue of edges, sorted according to a sort key.
Definition: SearchQueue.h:65
The vertex of the underlying graphs in gBITstar BIT*.
Definition: Vertex.h:68
Batch Informed Trees (BIT*)
Definition: BITstar.h:93
bool getPruning() const
Get whether graph and sample pruning is in use.
Definition: BITstar.cpp:1233
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
Definition: BITstar.cpp:477
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
Definition: BITstar.cpp:467
void setStopOnSolnImprovement(bool stopOnChange)
Stop the planner each time a solution improvement is found. Useful for examining the intermediate sol...
Definition: BITstar.cpp:1286
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
Definition: BITstar.cpp:1276
bool getStrictQueueOrdering() const
Get whether strict queue ordering is in use.
Definition: BITstar.cpp:1220
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1248
double getRewireFactor() const
Get the rewiring scale factor.
Definition: BITstar.cpp:1157
double getInitialInflationFactor() const
Get the inflation factor for the initial search.
Definition: BITstar.cpp:1127
std::shared_ptr< NearestNeighbors< VertexPtr > > VertexPtrNNPtr
The OMPL::NearestNeighbors structure.
Definition: BITstar.h:146
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
Definition: BITstar.cpp:1152
unsigned int getSamplesPerBatch() const
Get the number of samplers per batch.
Definition: BITstar.cpp:1167
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
Definition: BITstar.cpp:1315
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
Definition: BITstar.h:128
void setStrictQueueOrdering(bool beStrict)
Enable "strict sorting" of the edge queue. Rewirings can change the position in the queue of an edge....
Definition: BITstar.cpp:1214
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
Definition: BITstar.cpp:1271
void setInflationScalingParameter(double parameter)
The parameter that scales the inflation factor on the second search of each RGG approximation....
Definition: BITstar.cpp:1112
double getCurrentInflationFactor() const
Get the inflation factor for the current search.
Definition: BITstar.cpp:1142
void getPlannerData(base::PlannerData &data) const override
Get results.
Definition: BITstar.cpp:401
unsigned int numIterations() const
Get the number of iterations completed.
Definition: BITstar.cpp:472
double getCurrentTruncationFactor() const
Get the truncation factor for the current search.
Definition: BITstar.cpp:1147
void setPruning(bool prune)
Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the gra...
Definition: BITstar.cpp:1227
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
Definition: BITstar.cpp:1291
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:137
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:125
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:134
double getTruncationScalingParameter() const
Get the truncation factor parameter.
Definition: BITstar.cpp:1137
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:119
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
Definition: BITstar.cpp:1162
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:116
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
Definition: BITstar.cpp:1310
virtual ~BITstar() override=default
Destruct using the default destructor.
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition: BITstar.cpp:1321
ompl::base::Cost getNextEdgeValueInQueue()
Get the value of the next edge to be processed. Causes vertices in the queue to be expanded (if neces...
Definition: BITstar.cpp:447
void setInitialInflationFactor(double factor)
Set the inflation for the initial search of RGG approximation. See ABIT*'s class description for more...
Definition: BITstar.cpp:1106
void setJustInTimeSampling(bool useJit)
Delay the generation of samples until they are necessary. This only works when using an r-disc connec...
Definition: BITstar.cpp:1266
void clear() override
Clear the algorithm's internal state.
Definition: BITstar.cpp:270
void setup() override
Setup the algorithm.
Definition: BITstar.cpp:175
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: BITstar.cpp:1259
std::vector< VertexPtrPair > VertexPtrPairVector
A vector of pairs of vertices, i.e., a vector of edges.
Definition: BITstar.h:140
void enableCascadingRewirings(bool enable)
Enable the cascading of rewirings.
Definition: BITstar.cpp:1122
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1238
double getInflationScalingParameter() const
Get the inflation scaling parameter.
Definition: BITstar.cpp:1132
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: BITstar.cpp:1172
void setConsiderApproximateSolutions(bool findApproximate)
Set BIT* to consider approximate solutions during its initial search.
Definition: BITstar.cpp:1296
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:143
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
Definition: BITstar.cpp:1305
base::PlannerStatus solve(const base::PlannerTerminationCondition &terminationCondition) override
Solve the problem given a termination condition.
Definition: BITstar.cpp:307
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data.
Definition: BITstar.cpp:482
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:131
void setTruncationScalingParameter(double parameter)
Sets the parameter that scales the truncation factor for the searches of each RGG approximation....
Definition: BITstar.cpp:1117
void setDelayRewiringUntilInitialSolution(bool delayRewiring)
Delay the consideration of rewiring edges until an initial solution is found. When multiple batches a...
Definition: BITstar.cpp:1253
std::weak_ptr< Vertex > VertexWeakPtr
A weak pointer to a vertex.
Definition: BITstar.h:122
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:149
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: BITstar.cpp:1209
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition: BITstar.cpp:1281
std::pair< const ompl::base::State *, const ompl::base::State * > getNextEdgeInQueue()
Get the next edge to be processed. Causes vertices in the queue to be expanded (if necessary) and the...
Definition: BITstar.cpp:423
LogLevel
The set of priorities for message logging.
Definition: Console.h:85
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49