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 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_
38#define OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR_
39
40#include <algorithm>
41#include <memory>
42
43#include "ompl/base/Planner.h"
44#include "ompl/geometric/PathGeometric.h"
45#include "ompl/geometric/planners/informedtrees/aitstar/Edge.h"
46#include "ompl/geometric/planners/informedtrees/aitstar/ImplicitGraph.h"
47#include "ompl/geometric/planners/informedtrees/aitstar/Vertex.h"
48#include "ompl/geometric/planners/informedtrees/aitstar/Queuetypes.h"
49
50namespace ompl
51{
52 namespace geometric
53 {
85
88 {
89 public:
91 explicit AITstar(const ompl::base::SpaceInformationPtr &spaceInformation);
92
94 ~AITstar() = default;
95
97 void setup() override;
98
101
105
107 void clear() override;
108
111 solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override;
112
115
117 void getPlannerData(base::PlannerData &data) const override;
118
120 void setBatchSize(std::size_t batchSize);
121
123 std::size_t getBatchSize() const;
124
126 void setRewireFactor(double rewireFactor);
127
129 double getRewireFactor() const;
130
132 void trackApproximateSolutions(bool track);
133
136
138 void enablePruning(bool prune);
139
141 bool isPruningEnabled() const;
142
144 void setUseKNearest(bool useKNearest);
145
147 bool getUseKNearest() const;
148
150 void setMaxNumberOfGoals(unsigned int numberOfGoals);
151
153 unsigned int getMaxNumberOfGoals() const;
154
156 std::vector<aitstar::Edge> getEdgesInQueue() const;
157
159 std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInQueue() const;
160
163
165 std::shared_ptr<aitstar::Vertex> getNextVertexInQueue() const;
166
168 std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInReverseSearchTree() const;
169
170 private:
172 void iterate(const ompl::base::PlannerTerminationCondition &terminationCondition);
173
175 void iterateForwardSearch();
176
178 void iterateReverseSearch();
179
181 void updateReverseSearchVertex(const std::shared_ptr<aitstar::Vertex> &vertex);
182
184 void updateReverseSearchNeighbors(const std::shared_ptr<aitstar::Vertex> &vertex);
185
187 void insertOrUpdateInForwardQueue(const aitstar::Edge &edge);
188
190 void insertOrUpdateInForwardQueue(const std::vector<aitstar::Edge> &edges);
191
193 void insertOrUpdateInReverseQueue(const std::shared_ptr<aitstar::Vertex> &vertex);
194
196 void rebuildForwardQueue();
197
199 void rebuildReverseQueue();
200
202 void clearForwardQueue();
203
205 void clearReverseQueue();
206
208 void informAboutNewSolution() const;
209
211 void informAboutPlannerStatus(ompl::base::PlannerStatus::StatusType status) const;
212
214 void insertGoalVerticesInReverseQueue();
215
217 void expandStartVerticesIntoForwardQueue();
218
220 bool continueReverseSearch() const;
221
223 bool continueForwardSearch();
224
226 std::shared_ptr<ompl::geometric::PathGeometric>
227 getPathToVertex(const std::shared_ptr<aitstar::Vertex> &vertex) const;
228
230 std::array<ompl::base::Cost, 3u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &parent,
231 const std::shared_ptr<aitstar::Vertex> &child) const;
232
234 std::array<ompl::base::Cost, 2u> computeSortKey(const std::shared_ptr<aitstar::Vertex> &vertex) const;
235
237 void insertOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex);
238
240 std::vector<aitstar::Edge> getOutgoingEdges(const std::shared_ptr<aitstar::Vertex> &vertex) const;
241
244 void updateExactSolution();
245
248 void updateApproximateSolution(const std::shared_ptr<aitstar::Vertex> &vertex);
249
251 void updateApproximateSolution();
252
255
257 ompl::base::PlannerStatus::StatusType updateSolution(const std::shared_ptr<aitstar::Vertex> &vertex);
258
260 ompl::base::Cost computeCostToGoToStartHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
261
263 ompl::base::Cost computeCostToGoToGoalHeuristic(const std::shared_ptr<aitstar::Vertex> &vertex) const;
264
266 ompl::base::Cost computeCostToGoToGoal(const std::shared_ptr<aitstar::Vertex> &vertex) const;
267
269 ompl::base::Cost computeBestCostToComeFromGoalOfAnyStart() const;
270
272 std::size_t countNumVerticesInForwardTree() const;
273
275 std::size_t countNumVerticesInReverseTree() const;
276
279 void invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<aitstar::Vertex> &vertex);
280
282 ompl::base::Cost solutionCost_;
283
285 ompl::base::Cost approximateSolutionCost_{};
286
288 ompl::base::Cost approximateSolutionCostToGoal_{};
289
291 aitstar::ImplicitGraph graph_;
292
294 aitstar::EdgeQueue forwardQueue_;
295
297 aitstar::VertexQueue reverseQueue_;
298
300 bool isEdgeBetter(const aitstar::Edge &lhs, const aitstar::Edge &rhs) const;
301
303 bool isVertexBetter(const aitstar::KeyVertexPair &lhs, const aitstar::KeyVertexPair &rhs) const;
304
306 std::size_t numInconsistentOrUnconnectedTargets_{0u};
307
309 std::size_t numIterations_{0u};
310
312 std::size_t batchSize_{100u};
313
315 bool trackApproximateSolutions_{true};
316
318 bool isPruningEnabled_{true};
319
321 ompl::base::OptimizationObjectivePtr objective_;
322
324 ompl::base::MotionValidatorPtr motionValidator_;
325
327 std::size_t numProcessedEdges_{0u};
328
330 std::size_t numEdgeCollisionChecks_{0u};
331 };
332 } // namespace geometric
333} // namespace ompl
334
335#endif // OMPL_GEOMETRIC_PLANNERS_INFORMEDTREES_AITSTAR
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,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition AITstar.cpp:624
void enablePruning(bool prune)
Set whether pruning is enabled or not.
Definition AITstar.cpp:341
~AITstar()=default
Destructs a AIT*.
void setMaxNumberOfGoals(unsigned int numberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
Definition AITstar.cpp:361
double getRewireFactor() const
Get the rewire factor of the RGG graph.
Definition AITstar.cpp:318
bool isPruningEnabled() const
Get whether pruning is enabled or not.
Definition AITstar.cpp:346
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition AITstar.cpp:313
void setBatchSize(std::size_t batchSize)
Set the batch size.
Definition AITstar.cpp:303
ompl::base::PlannerStatus::StatusType ensureStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition)
Checks whether the problem is successfully setup.
Definition AITstar.cpp:169
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
Definition AITstar.cpp:254
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
Definition AITstar.cpp:366
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition AITstar.cpp:599
AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
Constructs a AIT*.
Definition AITstar.cpp:55
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition AITstar.cpp:356
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
Definition AITstar.cpp:614
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition AITstar.cpp:259
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition AITstar.cpp:94
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition AITstar.cpp:215
ompl::base::PlannerStatus::StatusType ensureSetup()
Checks whether the planner is successfully setup.
Definition AITstar.cpp:145
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition AITstar.cpp:634
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition AITstar.cpp:323
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:351
void clear() override
Clears the algorithm's internal state.
Definition AITstar.cpp:201
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
Definition AITstar.cpp:592
std::size_t getBatchSize() const
Get the batch size.
Definition AITstar.cpp:308
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition AITstar.cpp:336
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
StatusType
The possible values of the status returned by a planner.