Loading...
Searching...
No Matches
AITstar.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 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: Marlin Strub
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_
38#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_
39
40#include <algorithm>
41#include <memory>
42
43#include "ompl/base/Planner.h"
44#include "ompl/datastructures/BinaryHeap.h"
45#include "ompl/geometric/PathGeometric.h"
46#include "ompl/geometric/planners/informedtrees/aitstar/Edge.h"
47#include "ompl/geometric/planners/informedtrees/aitstar/ImplicitGraph.h"
48#include "ompl/geometric/planners/informedtrees/aitstar/Vertex.h"
49
50namespace ompl
51{
52 namespace geometric
53 {
88 {
89 public:
91 AITstar(const ompl::base::SpaceInformationPtr &spaceInformation);
92
94 ~AITstar() = default;
95
97 void setup() override;
98
100 void clear() override;
101
104 solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override;
105
108
110 void getPlannerData(base::PlannerData &data) const override;
111
113 void setBatchSize(std::size_t batchSize);
114
116 std::size_t getBatchSize() const;
117
119 void setRewireFactor(double rewireFactor);
120
122 double getRewireFactor() const;
123
125 void trackApproximateSolutions(bool track);
126
129
131 void enablePruning(bool prune);
132
134 bool isPruningEnabled() const;
135
137 void setUseKNearest(bool useKNearest);
138
140 bool getUseKNearest() const;
141
143 void setRepairReverseSearch(bool repairReverseSearch);
144
146 std::vector<aitstar::Edge> getEdgesInQueue() const;
147
149 std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInQueue() const;
150
153
155 std::shared_ptr<aitstar::Vertex> getNextVertexInQueue() const;
156
158 std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInReverseSearchTree() const;
159
160 private:
162 void iterate();
163
165 void performForwardSearchIteration();
166
168 void performReverseSearchIteration();
169
171 void reverseSearchUpdateVertex(const std::shared_ptr<aitstar::Vertex> &vertex);
172
174 void insertOrUpdateInForwardQueue(const aitstar::Edge &edge);
175
177 void insertOrUpdateInReverseQueue(const std::shared_ptr<aitstar::Vertex> &vertex);
178
180 void rebuildForwardQueue();
181
183 void rebuildReverseQueue();
184
186 void informAboutNewSolution() const;
187
189 void informAboutPlannerStatus(ompl::base::PlannerStatus::StatusType status) const;
190
192 std::shared_ptr<ompl::geometric::PathGeometric>
193 getPathToVertex(const std::shared_ptr<aitstar::Vertex> &vertex) const;
194
196 std::array<ompl::base::Cost, 3u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &parent,
197 const std::shared_ptr<aitstar::Vertex> &child) const;
198
200 std::array<ompl::base::Cost, 2u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &vertex) const;
201
203 void insertOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex);
204
206 std::vector<aitstar::Edge> getOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex) const;
207
209 bool haveAllVerticesBeenProcessed(const std::vector<aitstar::Edge> &edges) const;
210
213 bool haveAllVerticesBeenProcessed(const aitstar::Edge &edges) const;
214
217 void updateExactSolution();
218
221 void updateApproximateSolution(const std::shared_ptr<aitstar::Vertex> &vertex);
222
224 ompl::base::Cost computeCostToGoToStartHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
225
227 ompl::base::Cost computeCostToGoToGoalHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
228
230 ompl::base::Cost computeCostToGoToGoal(const std::shared_ptr<aitstar::Vertex> &vertex) const;
231
233 ompl::base::Cost computeBestCostToComeFromGoalOfAnyStart() const;
234
236 std::size_t countNumVerticesInForwardTree() const;
237
239 std::size_t countNumVerticesInReverseTree() const;
240
243 void invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<aitstar::Vertex> &vertex);
244
246 ompl::base::Cost solutionCost_;
247
249 ompl::base::Cost approximateSolutionCost_{};
250
252 ompl::base::Cost approximateSolutionCostToGoal_{};
253
255 aitstar::ImplicitGraph graph_;
256
258 using EdgeQueue =
259 ompl::BinaryHeap<aitstar::Edge, std::function<bool(const aitstar::Edge &, const aitstar::Edge &)>>;
260
262 EdgeQueue forwardQueue_;
263
265 using KeyVertexPair = std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>;
266
268 using VertexQueue =
269 ompl::BinaryHeap<KeyVertexPair, std::function<bool(const KeyVertexPair &, const KeyVertexPair &)>>;
270
272 VertexQueue reverseQueue_;
273
275 std::vector<aitstar::Edge> edgesToBeInserted_{};
276
278 std::size_t numIterations_{0u};
279
281 std::size_t batchSize_{100u};
282
284 bool performReverseSearchIteration_{true};
285
287 bool isForwardSearchStartedOnBatch_{false};
288
290 bool forwardQueueMustBeRebuilt_{false};
291
293 bool repairReverseSearch_{true};
294
296 bool trackApproximateSolutions_{true};
297
299 bool isPruningEnabled_{true};
300
303
305 ompl::base::MotionValidatorPtr motionValidator_;
306
308 std::size_t numProcessedEdges_{0u};
309
311 std::size_t numEdgeCollisionChecks_{0u};
312 };
313 } // namespace geometric
314} // namespace ompl
315
316#endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR
This class provides an implementation of an updatable min-heap. Using it is a bit cumbersome,...
Definition: BinaryHeap.h:53
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
A shared pointer wrapper for ompl::base::MotionValidator.
A shared pointer wrapper for ompl::base::OptimizationObjective.
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 shared pointer wrapper for ompl::base::SpaceInformation.
Adaptively Informed Trees (AIT*)
Definition: AITstar.h:88
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition: AITstar.cpp:504
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
Definition: AITstar.cpp:497
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition: AITstar.cpp:339
~AITstar()=default
Destructs a AIT*.
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
Definition: AITstar.cpp:519
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition: AITstar.cpp:316
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition: AITstar.cpp:344
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition: AITstar.cpp:311
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition: AITstar.cpp:539
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition: AITstar.cpp:301
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition: AITstar.cpp:251
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition: AITstar.cpp:354
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition: AITstar.cpp:256
void setRepairReverseSearch(bool repairReverseSearch)
Enable LPA* repair of reverse search.
Definition: AITstar.cpp:359
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition: AITstar.cpp:529
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition: AITstar.cpp:104
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition: AITstar.cpp:171
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition: AITstar.cpp:321
void setUseKNearest(bool useKNearest)
Set whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition: AITstar.cpp:349
void clear() override
Clears the algorithm's internal state.
Definition: AITstar.cpp:155
std::size_t getBatchSize() const
Get the batch size.
Definition: AITstar.cpp:306
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition: AITstar.cpp:334
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
StatusType
The possible values of the status returned by a planner.
Definition: PlannerStatus.h:52