ImplicitGraph.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019-present 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 setUseKNearest(bool useKNearest);
83 
85  bool getUseKNearest() const;
86 
88  void setTrackApproximateSolution(bool track);
89 
91  std::vector<std::shared_ptr<Vertex>> addSamples(std::size_t numNewSamples);
92 
94  std::size_t getNumVertices() const;
95 
97  double getConnectionRadius() const;
98 
100  void registerStartState(const ompl::base::State *const startState);
101 
103  void registerGoalState(const ompl::base::State *const goalState);
104 
106  bool hasAStartState() const;
107 
109  bool hasAGoalState() const;
110 
113  void updateStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition,
114  ompl::base::PlannerInputStates *inputStates);
115 
117  std::vector<std::shared_ptr<Vertex>> getNeighbors(const std::shared_ptr<Vertex> &vertex) const;
118 
120  bool isStart(const std::shared_ptr<Vertex> &vertex) const;
121 
123  bool isGoal(const std::shared_ptr<Vertex> &vertex) const;
124 
126  const std::vector<std::shared_ptr<Vertex>> &getStartVertices() const;
127 
129  const std::vector<std::shared_ptr<Vertex>> &getGoalVertices() const;
130 
132  std::vector<std::shared_ptr<Vertex>> getVertices() const;
133 
135  void prune();
136 
138  std::size_t getNumberOfSampledStates() const;
139 
141  std::size_t getNumberOfValidSamples() const;
142 
144  std::size_t getNumberOfStateCollisionChecks() const;
145 
147  std::size_t getNumberOfNearestNeighborCalls() const;
148 
149  private:
151  std::size_t computeNumberOfSamplesInInformedSet() const;
152 
154  double computeConnectionRadius(std::size_t numSamples) const;
155 
157  std::size_t computeNumberOfNeighbors(std::size_t numSamples) const;
158 
160  bool canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const;
161 
163  ompl::base::SpaceInformationPtr spaceInformation_;
164 
166  ompl::base::ProblemDefinitionPtr problemDefinition_;
167 
170 
172  std::size_t batchId_;
173 
175  double rewireFactor_{1.0};
176 
178  bool trackApproximateSolution_{false};
179 
181  std::shared_ptr<Vertex> bestApproximateGoal_;
182 
184  bool useKNearest_{true};
185 
187  double radius_{std::numeric_limits<double>::infinity()};
188 
191  std::size_t numNeighbors_{std::numeric_limits<std::size_t>::max()};
192 
194  std::size_t k_rgg_{std::numeric_limits<std::size_t>::max()};
195 
197  const ompl::base::Cost &solutionCost_;
198 
200  ompl::base::InformedSamplerPtr sampler_;
201 
204 
206  std::vector<std::shared_ptr<Vertex>> startVertices_;
207 
209  std::vector<std::shared_ptr<Vertex>> goalVertices_;
210 
214  std::vector<std::shared_ptr<Vertex>> prunedStartVertices_;
215 
219  std::vector<std::shared_ptr<Vertex>> prunedGoalVertices_;
220 
222  mutable std::size_t numValidSamples_{0u};
223 
225  mutable std::size_t numSampledStates_{0u};
226 
228  mutable std::size_t numNearestNeighborsCalls_{0u};
229  };
230 
231  } // namespace aitstar
232 
233  } // namespace geometric
234 
235 } // namespace ompl
236 
237 #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:142
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:114
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:112
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 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.
std::vector< std::shared_ptr< Vertex > > addSamples(std::size_t numNewSamples)
Adds a batch of samples and returns the samples it has added.
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.
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.
Main namespace. Contains everything in this library.
Definition: AppBase.h:22