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 
51 namespace ompl
52 {
53  namespace geometric
54  {
92  class BITstar : public ompl::base::Planner
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 = "kBITstar");
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 
189  ompl::base::Cost bestCost() const;
190 
192  unsigned int numBatches() const;
193 
194  // ---
195  // Settings.
196  // ---
197 
199  void setRewireFactor(double rewireFactor);
200 
202  double getRewireFactor() const;
203 
205  void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number);
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 
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 
293  bool getConsiderApproximateSolutions() const;
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 
480  std::shared_ptr<SearchQueue> queuePtr_{nullptr};
481 
483  double initialInflationFactor_{1.0};
484 
486  double truncationFactor_{1.0};
487 
489  double truncationScalingParameter_{0.0};
490 
492  double inflationScalingParameter_{0.0};
493 
495  VertexConstPtr curGoalVertex_{nullptr};
496 
499  // Gets set in setup to the proper calls from OptimizationObjective
500  ompl::base::Cost bestCost_;
501 
504  unsigned int bestLength_{0u};
505 
508  // Gets set in setup to the proper calls from OptimizationObjective.
509  ompl::base::Cost prunedCost_{std::numeric_limits<double>::infinity()};
510 
513  // Gets set in setup with the proper call to Planner::si_->getSpaceMeasure()
514  double prunedMeasure_{0.0};
515 
516  bool isFinalSearchOnBatch_{false};
517 
519  bool hasExactSolution_{false};
520 
522  bool isSearchDone_{false};
523 
525  bool stopLoop_{false};
526 
528  unsigned int numBatches_{0u};
529 
532  unsigned int numPrunings_{0u};
533 
535  unsigned int numIterations_{0u};
536 
538  unsigned int numRewirings_{0u};
539 
541  unsigned int numEdgeCollisionChecks_{0u};
542 
543  // ---
544  // Parameters - Set defaults in construction/setup and do not reset in clear.
545  // ---
546 
548  unsigned int samplesPerBatch_{100u};
549 
551  bool isPruningEnabled_{true};
552 
554  double pruneFraction_{0.05};
555 
557  bool stopOnSolutionChange_{false};
558  }; // class BITstar
559  } // namespace geometric
560 } // namespace ompl
561 
562 #endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_BITSTAR_
std::size_t getAverageNumOfAllowedFailedAttemptsWhenSampling() const
Get the average number of allowed failed attempts when sampling.
Definition: BITstar.cpp:1368
void setAverageNumOfAllowedFailedAttemptsWhenSampling(std::size_t number)
Set the average number of allowed failed attempts when sampling.
Definition: BITstar.cpp:1363
Base class for a planner.
Definition: Planner.h:279
std::vector< VertexPtrPair > VertexPtrPairVector
A vector of pairs of vertices, i.e., a vector of edges.
Definition: BITstar.h:236
ompl::base::Cost bestCost() const
Retrieve the best exact-solution cost found.
Definition: BITstar.cpp:534
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:1319
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:504
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:480
unsigned int numIterations() const
Get the number of iterations completed.
Definition: BITstar.cpp:529
std::shared_ptr< const Vertex > VertexConstPtr
A shared pointer to a const vertex.
Definition: BITstar.h:215
double getPruneThresholdFraction() const
Get the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1301
void clear() override
Clear the algorithm's internal state.
Definition: BITstar.cpp:327
void setup() override
Setup the algorithm.
Definition: BITstar.cpp:232
std::function< std::string()> NameFunc
A utility functor for ImplicitGraph and SearchQueue.
Definition: BITstar.h:245
std::pair< VertexConstPtr, VertexConstPtr > VertexConstPtrPair
A pair of const vertices, i.e., an edge.
Definition: BITstar.h:233
void setDropSamplesOnPrune(bool dropSamples)
Drop all unconnected samples when pruning, regardless of their heuristic value. This provides a metho...
Definition: BITstar.cpp:1329
void setInflationScalingParameter(double parameter)
The parameter that scales the inflation factor on the second search of each RGG approximation....
Definition: BITstar.cpp:1165
bool getConsiderApproximateSolutions() const
Get whether BIT* is considering approximate solutions.
Definition: BITstar.cpp:1358
double getInitialInflationFactor() const
Get the inflation factor for the initial search.
Definition: BITstar.cpp:1180
bool getStrictQueueOrdering() const
Get whether strict queue ordering is in use.
Definition: BITstar.cpp:1273
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
std::pair< VertexPtr, VertexPtr > VertexPtrPair
A pair of vertices, i.e., an edge.
Definition: BITstar.h:230
double getCurrentTruncationFactor() const
Get the truncation factor for the current search.
Definition: BITstar.cpp:1200
bool getUseKNearest() const
Get whether a k-nearest search is being used.
Definition: BITstar.cpp:1262
double getRewireFactor() const
Get the rewiring scale factor.
Definition: BITstar.cpp:1210
bool getStopOnSolnImprovement() const
Get whether BIT* stops each time a solution is found.
Definition: BITstar.cpp:1344
base::PlannerStatus solve(const base::PlannerTerminationCondition &terminationCondition) override
Solve the problem given a termination condition.
Definition: BITstar.cpp:364
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
unsigned int getSamplesPerBatch() const
Get the number of samplers per batch.
Definition: BITstar.cpp:1220
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setPruning(bool prune)
Enable pruning of vertices/samples that CANNOT improve the current solution. When a vertex in the gra...
Definition: BITstar.cpp:1280
LogLevel
The set of priorities for message logging.
Definition: Console.h:84
std::shared_ptr< NearestNeighbors< VertexPtr > > VertexPtrNNPtr
The OMPL::NearestNeighbors structure.
Definition: BITstar.h:242
void getPlannerData(base::PlannerData &data) const override
Get results.
Definition: BITstar.cpp:458
std::vector< VertexConstPtr > VertexConstPtrVector
A vector of shared pointers to const vertices.
Definition: BITstar.h:224
void setNearestNeighbors()
Set a different nearest neighbours datastructure.
Definition: BITstar.cpp:1374
A class to store the exit status of Planner::solve()
std::vector< VertexPtr > VertexPtrVector
A vector of shared pointers to vertices.
Definition: BITstar.h:221
BITstar(const base::SpaceInformationPtr &spaceInfo, const std::string &name="kBITstar")
Construct with a pointer to the space information and an optional name.
Definition: BITstar.cpp:129
bool getJustInTimeSampling() const
Get whether we're using just-in-time sampling.
Definition: BITstar.cpp:1324
void enableCascadingRewirings(bool enable)
Enable the cascading of rewirings.
Definition: BITstar.cpp:1175
void setStopOnSolnImprovement(bool stopOnChange)
Stop the planner each time a solution improvement is found. Useful for examining the intermediate sol...
Definition: BITstar.cpp:1339
std::shared_ptr< Vertex > VertexPtr
A shared pointer to a vertex.
Definition: BITstar.h:212
std::weak_ptr< Vertex > VertexWeakPtr
A weak pointer to a vertex.
Definition: BITstar.h:218
void setTruncationScalingParameter(double parameter)
Sets the parameter that scales the truncation factor for the searches of each RGG approximation....
Definition: BITstar.cpp:1170
double getCurrentInflationFactor() const
Get the inflation factor for the current search.
Definition: BITstar.cpp:1195
std::vector< VertexConstPtrPair > VertexConstPtrPairVector
A vector of pairs of const vertices, i.e., a vector of edges.
Definition: BITstar.h:239
void setConsiderApproximateSolutions(bool findApproximate)
Set BIT* to consider approximate solutions during its initial search.
Definition: BITstar.cpp:1349
virtual ~BITstar() override=default
Destruct using the default destructor.
bool getDelayRewiringUntilInitialSolution() const
Get whether BIT* is delaying rewiring until a solution is found.
Definition: BITstar.cpp:1312
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:1267
void setUseKNearest(bool useKNearest)
Enable a k-nearest search for instead of an r-disc search.
Definition: BITstar.cpp:1225
double getInflationScalingParameter() const
Get the inflation scaling parameter.
Definition: BITstar.cpp:1185
double getTruncationScalingParameter() const
Get the truncation factor parameter.
Definition: BITstar.cpp:1190
void setDelayRewiringUntilInitialSolution(bool delayRewiring)
Delay the consideration of rewiring edges until an initial solution is found. When multiple batches a...
Definition: BITstar.cpp:1306
unsigned int VertexId
The vertex id type.
Definition: BITstar.h:227
bool getPruning() const
Get whether graph and sample pruning is in use.
Definition: BITstar.cpp:1286
void getEdgeQueue(VertexConstPtrPairVector *edgesInQueue)
Get the whole messy set of edges in the queue. Expensive but helpful for some videos.
Definition: BITstar.cpp:524
void setSamplesPerBatch(unsigned int n)
Set the number of samplers per batch.
Definition: BITstar.cpp:1215
unsigned int numBatches() const
Retrieve the number of batches processed as the raw data.
Definition: BITstar.cpp:539
void setPruneThresholdFraction(double fractionalChange)
Set the fractional change in the solution cost AND problem measure necessary for pruning to occur.
Definition: BITstar.cpp:1291
void setRewireFactor(double rewireFactor)
Set the rewiring scale factor, s, such that r_rrg = s \times r_rrg*.
Definition: BITstar.cpp:1205
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setInitialInflationFactor(double factor)
Set the inflation for the initial search of RGG approximation. See ABIT*'s class description for more...
Definition: BITstar.cpp:1159
bool getDropSamplesOnPrune() const
Get whether unconnected samples are dropped on pruning.
Definition: BITstar.cpp:1334