ImplicitGraph.cpp
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#include "ompl/geometric/planners/informedtrees/aitstar/ImplicitGraph.h"
37
38#include <cmath>
39
40#include <boost/math/constants/constants.hpp>
41
42#include "ompl/util/GeometricEquations.h"
43
44namespace ompl
45{
46 namespace geometric
47 {
48 namespace aitstar
49 {
51 : batchId_(1u), solutionCost_(solutionCost)
52 {
53 }
54
56 const ompl::base::ProblemDefinitionPtr &problemDefinition,
58 {
59 vertices_.setDistanceFunction(
60 [this](const std::shared_ptr<Vertex> &a, const std::shared_ptr<Vertex> &b) {
61 return spaceInformation_->distance(a->getState(), b->getState());
62 });
63 spaceInformation_ = spaceInformation;
64 problemDefinition_ = problemDefinition;
65 objective_ = problemDefinition->getOptimizationObjective();
66 k_rgg_ = boost::math::constants::e<double>() +
67 (boost::math::constants::e<double>() / spaceInformation->getStateDimension());
69 }
70
72 {
73 batchId_ = 1u;
74 radius_ = std::numeric_limits<double>::infinity();
75 numNeighbors_ = std::numeric_limits<std::size_t>::max();
76 vertices_.clear();
77 startVertices_.clear();
78 goalVertices_.clear();
79 prunedStartVertices_.clear();
80 prunedGoalVertices_.clear();
81 numSampledStates_ = 0u;
82 numValidSamples_ = 0u;
83 }
84
85 void ImplicitGraph::setRewireFactor(double rewireFactor)
86 {
87 rewireFactor_ = rewireFactor;
88 }
89
91 {
92 return rewireFactor_;
93 }
94
95 void ImplicitGraph::setUseKNearest(bool useKNearest)
96 {
97 useKNearest_ = useKNearest;
98 }
99
101 {
102 return useKNearest_;
103 }
104
106 {
107 // Create a vertex corresponding to this state.
108 auto startVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
109
110 // Copy the state into the vertex's state.
111 spaceInformation_->copyState(startVertex->getState(), startState);
112
113 // By definition, this has identity cost-to-come.
114 startVertex->setCostToComeFromStart(objective_->identityCost());
115
116 // Add the start vertex to the set of vertices.
117 vertices_.add(startVertex);
118
119 // Remember it as a start vertex.
120 startVertices_.emplace_back(startVertex);
121 }
122
124 {
125 // Create a vertex corresponding to this state.
126 auto goalVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
127
128 // Copy the state into the vertex's state.
129 spaceInformation_->copyState(goalVertex->getState(), goalState);
130
131 // Add the goal vertex to the set of vertices.
132 vertices_.add(goalVertex);
133
134 // Remember it as a goal vertex.
135 goalVertices_.emplace_back(goalVertex);
136 }
137
139 {
140 return !startVertices_.empty();
141 }
142
144 {
145 return !goalVertices_.empty();
146 }
147
148 void
151 {
152 // We need to keep track whether a new goal and/or a new start has been added.
153 bool addedNewGoalState = false;
154 bool addedNewStartState = false;
155
156 // First update the goals. We have to call inputStates->nextGoal(terminationCondition) at least once
157 // (regardless of the return value of inputStates->moreGoalStates()) in case the termination condition
158 // wants us to wait for a goal.
159 do
160 {
161 // Get a new goal. If there are none, or the underlying state is invalid this will be a nullptr.
162 auto newGoalState = inputStates->nextGoal(terminationCondition);
163
164 // If there was a new valid goal, register it as such and remember that a goal has been added.
165 if (static_cast<bool>(newGoalState))
166 {
167 registerGoalState(newGoalState);
168 addedNewGoalState = true;
169 }
170
171 } while (inputStates->haveMoreGoalStates());
172
173 // Having updated the goals, we now update the starts.
174 while (inputStates->haveMoreStartStates())
175 {
176 // Get the next start. The returned pointer can be a nullptr (if the state is invalid).
177 auto newStartState = inputStates->nextStart();
178
179 // If there is a new valid start, register it as such and remember that a start has been added.
180 if (static_cast<bool>(newStartState))
181 {
182 registerStartState(newStartState);
183 addedNewStartState = true;
184 }
185 }
186
187 // If we added a new start and have previously pruned goals, we might want to add the goals back.
188 if (addedNewStartState && !prunedGoalVertices_.empty())
189 {
190 // Keep track of the pruned goal vertices that have been revived.
191 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedGoals;
192
193 // Let's see if the pruned goal is close enough to any new start to revive it..
194 for (auto it = prunedGoalVertices_.begin(); it != prunedGoalVertices_.end(); ++it)
195 {
196 // Loop over all start states to get the best cost.
197 auto heuristicCost = objective_->infiniteCost();
198 for (const auto &start : startVertices_)
199 {
200 heuristicCost = objective_->betterCost(
201 heuristicCost, objective_->motionCostHeuristic(start->getState(), (*it)->getState()));
202 }
203
204 // If this goal can possibly improve the current solution, add it back to the graph.
205 if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
206 {
207 registerGoalState((*it)->getState());
208 addedNewGoalState = true;
209 revivedGoals.emplace_back(it);
210 }
211 }
212
213 // Remove all revived goals from the pruned goals.
214 for (const auto &revivedGoal : revivedGoals)
215 {
216 std::iter_swap(revivedGoal, prunedGoalVertices_.rbegin());
217 prunedGoalVertices_.pop_back();
218 }
219 }
220
221 // If we added a new goal and have previously pruned starts, we might want to add the starts back.
222 if (addedNewGoalState && !prunedStartVertices_.empty())
223 {
224 // Keep track of the pruned goal vertices that have been revived.
225 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedStarts;
226
227 // Let's see if the pruned start is close enough to any new goal to revive it..
228 for (auto it = prunedStartVertices_.begin(); it != prunedStartVertices_.end(); ++it)
229 {
230 // Loop over all start states to get the best cost.
231 auto heuristicCost = objective_->infiniteCost();
232 for (const auto &goal : goalVertices_)
233 {
234 heuristicCost = objective_->betterCost(
235 heuristicCost, objective_->motionCostHeuristic(goal->getState(), (*it)->getState()));
236 }
237
238 // If this goal can possibly improve the current solution, add it back to the graph.
239 if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
240 {
241 registerStartState((*it)->getState());
242 addedNewStartState = true;
243 revivedStarts.emplace_back(it);
244 }
245 }
246
247 // Remove all revived goals from the pruned goals.
248 for (const auto &revivedStart : revivedStarts)
249 {
250 std::iter_swap(revivedStart, prunedStartVertices_.rbegin());
251 prunedStartVertices_.pop_back();
252 }
253 }
254
255 if (addedNewGoalState || addedNewStartState)
256 {
257 // Allocate a state sampler if we have at least one start and one goal.
258 if (!startVertices_.empty() && !goalVertices_.empty())
259 {
260 sampler_ = objective_->allocInformedStateSampler(problemDefinition_,
261 std::numeric_limits<unsigned int>::max());
262 }
263 }
264
265 if (!goalVertices_.empty() && startVertices_.empty())
266 {
267 OMPL_WARN("AIT* (ImplicitGraph): The problem has a goal but not a start. AIT* can not find a "
268 "solution since PlannerInputStates provides no method to wait for a valid start state to "
269 "appear.");
270 }
271 }
272
273 std::size_t ImplicitGraph::computeNumberOfSamplesInInformedSet() const
274 {
275 std::size_t numberOfSamplesInInformedSet{0u};
276 std::vector<std::shared_ptr<Vertex>> vertices;
277 vertices_.list(vertices);
278
279 // Loop over all vertices.
280 for (const auto &vertex : vertices)
281 {
282 // Get the best cost to come from any start.
283 ompl::base::Cost bestCostToComeHeuristic = objective_->infiniteCost();
284 for (const auto &start : startVertices_)
285 {
286 auto costToComeHeuristic =
287 objective_->motionCostHeuristic(start->getState(), vertex->getState());
288 if (objective_->isCostBetterThan(costToComeHeuristic, bestCostToComeHeuristic))
289 {
290 bestCostToComeHeuristic = costToComeHeuristic;
291 }
292 }
293
294 // Get the best cost to go to any goal.
295 ompl::base::Cost bestCostToGoHeuristic = objective_->infiniteCost();
296 for (const auto &goal : goalVertices_)
297 {
298 auto costToComeHeuristic =
299 objective_->motionCostHeuristic(vertex->getState(), goal->getState());
300 if (objective_->isCostBetterThan(costToComeHeuristic, bestCostToGoHeuristic))
301 {
302 bestCostToGoHeuristic = costToComeHeuristic;
303 }
304 }
305
306 // If this can possibly improve the current solution, it is in the informed set.
307 if (objective_->isCostBetterThan(
308 objective_->combineCosts(bestCostToComeHeuristic, bestCostToGoHeuristic), solutionCost_))
309 {
310 ++numberOfSamplesInInformedSet;
311 }
312 }
313
314 return numberOfSamplesInInformedSet;
315 }
316
317 std::vector<std::shared_ptr<Vertex>> ImplicitGraph::addSamples(std::size_t numNewSamples)
318 {
319 // First get the number of samples inside the informed set.
320 auto numSamplesInInformedSet = computeNumberOfSamplesInInformedSet();
321
322 // Create new vertices.
323 std::vector<std::shared_ptr<Vertex>> newVertices;
324 newVertices.reserve(numNewSamples);
325 while (newVertices.size() < numNewSamples)
326 {
327 // Create a new vertex.
328 newVertices.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
329
330 do
331 {
332 // Sample the associated state uniformly within the informed set.
333 sampler_->sampleUniform(newVertices.back()->getState(), solutionCost_);
334
335 // Count how many states we've checked.
336 ++numSampledStates_;
337 } while (!spaceInformation_->getStateValidityChecker()->isValid(newVertices.back()->getState()));
338
339 ++numValidSamples_;
340 }
341
342 // Add all new vertices to the nearest neighbor structure.
343 vertices_.add(newVertices);
344
345 auto numUniformSamplesInInformedSet =
346 numSamplesInInformedSet + numNewSamples - startVertices_.size() - goalVertices_.size();
347
348 // We need to do some internal housekeeping.
349 ++batchId_;
350 if (useKNearest_)
351 {
352 numNeighbors_ = computeNumberOfNeighbors(numUniformSamplesInInformedSet);
353 }
354 else
355 {
356 radius_ = computeConnectionRadius(numUniformSamplesInInformedSet);
357 }
358
359 return newVertices;
360 }
361
363 {
364 return vertices_.size();
365 }
366
368 {
369 return radius_;
370 }
371
372 std::vector<std::shared_ptr<Vertex>>
373 ImplicitGraph::getNeighbors(const std::shared_ptr<Vertex> &vertex) const
374 {
375 // Return cached neighbors if available.
376 if (vertex->hasCachedNeighbors())
377 {
378 return vertex->getNeighbors();
379 }
380 else
381 {
382 ++numNearestNeighborsCalls_;
383 std::vector<std::shared_ptr<Vertex>> neighbors{};
384 if (useKNearest_)
385 {
386 vertices_.nearestK(vertex, numNeighbors_, neighbors);
387 }
388 else
389 {
390 vertices_.nearestR(vertex, radius_, neighbors);
391 }
392 vertex->cacheNeighbors(neighbors);
393 return neighbors;
394 }
395 }
396
397 bool ImplicitGraph::isStart(const std::shared_ptr<Vertex> &vertex) const
398 {
399 for (const auto &start : startVertices_)
400 {
401 if (vertex->getId() == start->getId())
402 {
403 return true;
404 }
405 }
406 return false;
407 }
408
409 bool ImplicitGraph::isGoal(const std::shared_ptr<Vertex> &vertex) const
410 {
411 for (const auto &goal : goalVertices_)
412 {
413 if (vertex->getId() == goal->getId())
414 {
415 return true;
416 }
417 }
418 return false;
419 }
420
421 const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getStartVertices() const
422 {
423 return startVertices_;
424 }
425
426 const std::vector<std::shared_ptr<Vertex>> &ImplicitGraph::getGoalVertices() const
427 {
428 return goalVertices_;
429 }
430
431 std::vector<std::shared_ptr<Vertex>> ImplicitGraph::getVertices() const
432 {
433 std::vector<std::shared_ptr<Vertex>> vertices;
434 vertices_.list(vertices);
435 return vertices;
436 }
437
439 {
440 if (!objective_->isFinite(solutionCost_))
441 {
442 return;
443 }
444
445 std::vector<std::shared_ptr<Vertex>> vertices;
446 vertices_.list(vertices);
447
448 // Prepare the vector of vertices to be pruned.
449 std::vector<std::shared_ptr<Vertex>> verticesToBePruned;
450
451 // Check each vertex whether it can be pruned.
452 for (const auto &vertex : vertices)
453 {
454 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
455 // that is more expensive than the current solution.
456 if (!canPossiblyImproveSolution(vertex))
457 {
458 // We keep track of pruned start and goal vertices. This is because if the user adds start or
459 // goal states after we have pruned start or goal states, we might want to reconsider pruned
460 // start or goal states.
461 if (isGoal(vertex))
462 {
463 prunedGoalVertices_.emplace_back(vertex);
464 }
465 else if (isStart(vertex))
466 {
467 prunedStartVertices_.emplace_back(vertex);
468 }
469 verticesToBePruned.emplace_back(vertex);
470 }
471 }
472
473 // Remove all vertices to be pruned.
474 for (const auto &vertex : verticesToBePruned)
475 {
476 // Remove it from both search trees.
477 if (vertex->hasReverseParent())
478 {
479 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
480 vertex->resetReverseParent();
481 }
482 vertex->invalidateReverseBranch();
483 if (vertex->hasForwardParent())
484 {
485 vertex->getForwardParent()->removeFromForwardChildren(vertex->getId());
486 vertex->resetForwardParent();
487 }
488 vertex->invalidateForwardBranch();
489
490 // Remove it from the nearest neighbor struct.
491 vertices_.remove(vertex);
492 }
493
494 // Assert that the forward and reverse queue are empty?
495 }
496
498 {
499 return numSampledStates_;
500 }
501
503 {
504 return numValidSamples_;
505 }
506
508 {
509 // Each sampled state is checked for collision. Only sampled states are checked for collision (number of
510 // collision checked edges don't count here.)
511 return numSampledStates_;
512 }
513
515 {
516 return numNearestNeighborsCalls_;
517 }
518
519 double ImplicitGraph::computeConnectionRadius(std::size_t numSamples) const
520 {
521 // Define the dimension as a helper variable.
522 auto dimension = static_cast<double>(spaceInformation_->getStateDimension());
523
524 // Compute the RRT* factor.
525 return rewireFactor_ *
526 std::pow(2.0 * (1.0 + 1.0 / dimension) *
527 (sampler_->getInformedMeasure(solutionCost_) /
528 unitNBallMeasure(spaceInformation_->getStateDimension())) *
529 (std::log(static_cast<double>(numSamples)) / static_cast<double>(numSamples)),
530 1.0 / dimension);
531
532 // // Compute the FMT* factor.
533 // return 2.0 * rewireFactor_ *
534 // std::pow((1.0 / dimension) *
535 // (sampler_->getInformedMeasure(*solutionCost_.lock()) /
536 // unitNBallMeasure(spaceInformation_->getStateDimension())) *
537 // (std::log(static_cast<double>(numSamples)) / numSamples),
538 // 1.0 / dimension);
539 }
540
541 std::size_t ImplicitGraph::computeNumberOfNeighbors(std::size_t numSamples) const
542 {
543 return std::ceil(rewireFactor_ * k_rgg_ * std::log(static_cast<double>(numSamples)));
544 }
545
546 bool ImplicitGraph::canPossiblyImproveSolution(const std::shared_ptr<Vertex> &vertex) const
547 {
548 // Get the preferred start for this vertex.
549 auto bestCostToCome = objective_->infiniteCost();
550 for (const auto &start : startVertices_)
551 {
552 auto costToCome = objective_->motionCostHeuristic(start->getState(), vertex->getState());
553 if (objective_->isCostBetterThan(costToCome, bestCostToCome))
554 {
555 bestCostToCome = costToCome;
556 }
557 }
558
559 // Check if the combination of the admissible costToCome and costToGo estimates results in a path
560 // that is more expensive than the current solution.
561 return objective_->isCostBetterThan(
562 objective_->combineCosts(
563 bestCostToCome, objective_->costToGo(vertex->getState(), problemDefinition_->getGoal().get())),
564 solutionCost_);
565 }
566
567 } // namespace aitstar
568
569 } // namespace geometric
570
571} // namespace ompl
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or nullptr if no more valid goal states are available....
Definition: Planner.cpp:274
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A shared pointer wrapper for ompl::base::ProblemDefinition.
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
bool hasAStartState() const
Returns whether the graph has a goal state.
std::size_t getNumberOfNearestNeighborCalls() const
Get the number of nearest neighbor calls.
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
double getRewireFactor() const
Get the reqire factor of the RGG.
std::size_t getNumberOfStateCollisionChecks() const
Get the number of state collision checks.
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
void prune()
Prune all samples that can not contribute to a solution better than the current one.
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.
std::vector< std::shared_ptr< Vertex > > addSamples(std::size_t numNewSamples)
Adds a batch of samples and returns the samples it has added.
void registerStartState(const ompl::base::State *const startState)
Registers a state as a start state.
void clear()
Resets the graph to its construction state, without resetting options.
void registerGoalState(const ompl::base::State *const goalState)
Registers a state as a goal state.
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
double getConnectionRadius() const
Gets the RGG connection radius.
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.
ImplicitGraph(const ompl::base::Cost &solutionCost)
Constructs an implicit graph.
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
std::size_t getNumVertices() const
Gets the number of samples in the graph.
bool hasAGoalState() const
Returns whether the graph has a goal state.
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
Main namespace. Contains everything in this library.
double unitNBallMeasure(unsigned int N)
The Lebesgue measure (i.e., "volume") of an n-dimensional ball with a unit radius.