ImplicitGraph.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Oxford
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 names of the copyright holders 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: Marlin Strub
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_IMPLICITGRAPH_
38 #define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_IMPLICITGRAPH_
39 
40 #include <memory>
41 
42 #include "ompl/base/samplers/InformedStateSampler.h"
43 #include "ompl/base/SpaceInformation.h"
44 #include "ompl/base/ProblemDefinition.h"
45 #include "ompl/base/PlannerTerminationCondition.h"
46 #include "ompl/base/Planner.h"
47 
48 #include "ompl/datastructures/NearestNeighborsGNATNoThreadSafety.h"
49 
50 #include "ompl/geometric/planners/informedtrees/aitstar/Vertex.h"
51 
52 namespace ompl
53 {
54  namespace geometric
55  {
56  namespace aitstar
57  {
58  class ImplicitGraph
59  {
60  public:
62  ImplicitGraph(const ompl::base::Cost &solutionCost);
63 
65  virtual ~ImplicitGraph() = default;
66 
68  void setup(const ompl::base::SpaceInformationPtr &spaceInformation,
69  const ompl::base::ProblemDefinitionPtr &problemDefinition,
70  ompl::base::PlannerInputStates *inputStates);
71 
73  void clear();
74 
76  void setRewireFactor(double rewireFactor);
77 
79  double getRewireFactor() const;
80 
82  void setMaxNumberOfGoals(unsigned int maxNumberOfGoals);
83 
85  unsigned int getMaxNumberOfGoals() const;
86 
88  void setUseKNearest(bool useKNearest);
89 
91  bool getUseKNearest() const;
92 
94  void setTrackApproximateSolution(bool track);
95 
97  bool addSamples(std::size_t numNewSamples,
98  const ompl::base::PlannerTerminationCondition &terminationCondition);
99 
101  std::size_t getNumVertices() const;
102 
104  double getConnectionRadius() const;
105 
107  void registerStartState(const ompl::base::State *const startState);
108 
110  void registerGoalState(const ompl::base::State *const goalState);
111 
113  bool hasAStartState() const;
114 
116  bool hasAGoalState() const;
117 
120  void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition,
121  ompl::base::PlannerInputStates *inputStates);
122 
124  std::vector<std::shared_ptr<Vertex>> getNeighbors(const std::shared_ptr<Vertex> &vertex) const;
125 
127  bool isStart(const std::shared_ptr<Vertex> &vertex) const;
128 
130  bool isGoal(const std::shared_ptr<Vertex> &vertex) const;
131 
133  const std::vector<std::shared_ptr<Vertex>> &getStartVertices() const;
134 
136  const std::vector<std::shared_ptr<Vertex>> &getGoalVertices() const;
137 
139  std::vector<std::shared_ptr<Vertex>> getVertices() const;
140 
142  void prune();
143 
145  std::size_t getNumberOfSampledStates() const;
146 
148  std::size_t getNumberOfValidSamples() const;
149 
151  std::size_t getNumberOfStateCollisionChecks() const;
152 
154  std::size_t getNumberOfNearestNeighborCalls() const;
155 
156  private:
158  std::size_t computeNumberOfSamplesInInformedSet() const;
159 
161  double computeConnectionRadius(std::size_t numSamples) const;
162 
164  std::size_t computeNumberOfNeighbors(std::size_t numSamples) const;
165 
167  bool canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const;
168 
170  ompl::base::SpaceInformationPtr spaceInformation_;
171 
173  ompl::base::ProblemDefinitionPtr problemDefinition_;
174 
177 
179  std::size_t batchId_;
180 
182  double rewireFactor_{1.0};
183 
185  bool trackApproximateSolution_{false};
186 
188  std::shared_ptr<Vertex> bestApproximateGoal_;
189 
191  bool useKNearest_{true};
192 
194  unsigned int maxNumGoals_{10u};
195 
197  double radius_{std::numeric_limits<double>::infinity()};
198 
201  std::size_t numNeighbors_{std::numeric_limits<std::size_t>::max()};
202 
204  std::size_t k_rgg_{std::numeric_limits<std::size_t>::max()};
205 
207  const ompl::base::Cost &solutionCost_;
208 
210  ompl::base::InformedSamplerPtr sampler_;
211 
214 
216  std::vector<std::shared_ptr<Vertex>> startVertices_;
217 
219  std::vector<std::shared_ptr<Vertex>> goalVertices_;
220 
224  std::vector<std::shared_ptr<Vertex>> prunedStartVertices_;
225 
229  std::vector<std::shared_ptr<Vertex>> prunedGoalVertices_;
230 
232  std::vector<std::shared_ptr<Vertex>> newSamples_;
233 
235  mutable std::size_t numValidSamples_{0u};
236 
238  mutable std::size_t numSampledStates_{0u};
239 
241  mutable std::size_t numNearestNeighborsCalls_{0u};
242  };
243 
244  } // namespace aitstar
245 
246  } // namespace geometric
247 
248 } // namespace ompl
249 
250 #endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_IMPLICITGRAPH_
bool hasAStartState() const
Returns whether the graph has a goal state.
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:141
void setup(const ompl::base::SpaceInformationPtr &spaceInformation, const ompl::base::ProblemDefinitionPtr &problemDefinition, ompl::base::PlannerInputStates *inputStates)
The setup method for the graph. Needed to have it on the stack.
std::size_t getNumberOfNearestNeighborCalls() const
Get the number of nearest neighbor calls.
void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition, ompl::base::PlannerInputStates *inputStates)
Adds new start and goals to the graph if avavilable and creates a new informed sampler if necessary.
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
Definition of an abstract state.
Definition: State.h:113
Geometric Near-neighbor Access Tree (GNAT), a data structure for nearest neighbor search.
bool hasAGoalState() const
Returns whether the graph has a goal state.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
void registerStartState(const ompl::base::State *const startState)
Registers a state as a start state.
A shared pointer wrapper for ompl::base::OptimizationObjective.
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
void setMaxNumberOfGoals(unsigned int maxNumberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
void registerGoalState(const ompl::base::State *const goalState)
Registers a state as a goal state.
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void clear()
Resets the graph to its construction state, without resetting options.
double getRewireFactor() const
Get the reqire factor of the RGG.
virtual ~ImplicitGraph()=default
Destructs an implicit graph.
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
A shared pointer wrapper for ompl::base::ProblemDefinition.
double getConnectionRadius() const
Gets the RGG connection radius.
std::size_t getNumVertices() const
Gets the number of samples in the graph.
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
void prune()
Prune all samples that can not contribute to a solution better than the current one.
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
void setTrackApproximateSolution(bool track)
Sets whether to track approximate solutions or not.
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
bool addSamples(std::size_t numNewSamples, const ompl::base::PlannerTerminationCondition &terminationCondition)
Adds a batch of samples and returns the samples it has added.
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
ImplicitGraph(const ompl::base::Cost &solutionCost)
Constructs an implicit graph.
std::size_t getNumberOfStateCollisionChecks() const
Get the number of state collision checks.
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21