36#include "ompl/geometric/planners/informedtrees/aitstar/ImplicitGraph.h"
40#include <boost/math/constants/constants.hpp>
42#include "ompl/util/GeometricEquations.h"
51 : batchId_(1u), solutionCost_(solutionCost)
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());
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());
74 radius_ = std::numeric_limits<double>::infinity();
75 numNeighbors_ = std::numeric_limits<std::size_t>::max();
77 startVertices_.clear();
78 goalVertices_.clear();
79 prunedStartVertices_.clear();
80 prunedGoalVertices_.clear();
81 numSampledStates_ = 0u;
82 numValidSamples_ = 0u;
87 rewireFactor_ = rewireFactor;
97 useKNearest_ = useKNearest;
108 auto startVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
111 spaceInformation_->copyState(startVertex->getState(), startState);
114 startVertex->setCostToComeFromStart(objective_->identityCost());
117 vertices_.add(startVertex);
120 startVertices_.emplace_back(startVertex);
126 auto goalVertex = std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_);
129 spaceInformation_->copyState(goalVertex->getState(), goalState);
132 vertices_.add(goalVertex);
135 goalVertices_.emplace_back(goalVertex);
140 return !startVertices_.empty();
145 return !goalVertices_.empty();
153 bool addedNewGoalState =
false;
154 bool addedNewStartState =
false;
162 auto newGoalState = inputStates->
nextGoal(terminationCondition);
165 if (
static_cast<bool>(newGoalState))
168 addedNewGoalState =
true;
177 auto newStartState = inputStates->
nextStart();
180 if (
static_cast<bool>(newStartState))
183 addedNewStartState =
true;
188 if (addedNewStartState && !prunedGoalVertices_.empty())
191 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedGoals;
194 for (
auto it = prunedGoalVertices_.begin(); it != prunedGoalVertices_.end(); ++it)
197 auto heuristicCost = objective_->infiniteCost();
198 for (
const auto &start : startVertices_)
200 heuristicCost = objective_->betterCost(
201 heuristicCost, objective_->motionCostHeuristic(start->getState(), (*it)->getState()));
205 if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
208 addedNewGoalState =
true;
209 revivedGoals.emplace_back(it);
214 for (
const auto &revivedGoal : revivedGoals)
216 std::iter_swap(revivedGoal, prunedGoalVertices_.rbegin());
217 prunedGoalVertices_.pop_back();
222 if (addedNewGoalState && !prunedStartVertices_.empty())
225 std::vector<std::vector<std::shared_ptr<Vertex>>::iterator> revivedStarts;
228 for (
auto it = prunedStartVertices_.begin(); it != prunedStartVertices_.end(); ++it)
231 auto heuristicCost = objective_->infiniteCost();
232 for (
const auto &goal : goalVertices_)
234 heuristicCost = objective_->betterCost(
235 heuristicCost, objective_->motionCostHeuristic(goal->getState(), (*it)->getState()));
239 if (objective_->isCostBetterThan(heuristicCost, solutionCost_))
242 addedNewStartState =
true;
243 revivedStarts.emplace_back(it);
248 for (
const auto &revivedStart : revivedStarts)
250 std::iter_swap(revivedStart, prunedStartVertices_.rbegin());
251 prunedStartVertices_.pop_back();
255 if (addedNewGoalState || addedNewStartState)
258 if (!startVertices_.empty() && !goalVertices_.empty())
260 sampler_ = objective_->allocInformedStateSampler(problemDefinition_,
261 std::numeric_limits<unsigned int>::max());
265 if (!goalVertices_.empty() && startVertices_.empty())
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 "
273 std::size_t ImplicitGraph::computeNumberOfSamplesInInformedSet()
const
275 std::size_t numberOfSamplesInInformedSet{0u};
276 std::vector<std::shared_ptr<Vertex>> vertices;
277 vertices_.list(vertices);
280 for (
const auto &vertex : vertices)
284 for (
const auto &start : startVertices_)
286 auto costToComeHeuristic =
287 objective_->motionCostHeuristic(start->getState(), vertex->getState());
288 if (objective_->isCostBetterThan(costToComeHeuristic, bestCostToComeHeuristic))
290 bestCostToComeHeuristic = costToComeHeuristic;
296 for (
const auto &goal : goalVertices_)
298 auto costToComeHeuristic =
299 objective_->motionCostHeuristic(vertex->getState(), goal->getState());
300 if (objective_->isCostBetterThan(costToComeHeuristic, bestCostToGoHeuristic))
302 bestCostToGoHeuristic = costToComeHeuristic;
307 if (objective_->isCostBetterThan(
308 objective_->combineCosts(bestCostToComeHeuristic, bestCostToGoHeuristic), solutionCost_))
310 ++numberOfSamplesInInformedSet;
314 return numberOfSamplesInInformedSet;
320 auto numSamplesInInformedSet = computeNumberOfSamplesInInformedSet();
323 std::vector<std::shared_ptr<Vertex>> newVertices;
324 newVertices.reserve(numNewSamples);
325 while (newVertices.size() < numNewSamples)
328 newVertices.emplace_back(std::make_shared<Vertex>(spaceInformation_, problemDefinition_, batchId_));
333 sampler_->sampleUniform(newVertices.back()->getState(), solutionCost_);
337 }
while (!spaceInformation_->getStateValidityChecker()->isValid(newVertices.back()->getState()));
343 vertices_.add(newVertices);
345 auto numUniformSamplesInInformedSet =
346 numSamplesInInformedSet + numNewSamples - startVertices_.size() - goalVertices_.size();
352 numNeighbors_ = computeNumberOfNeighbors(numUniformSamplesInInformedSet);
356 radius_ = computeConnectionRadius(numUniformSamplesInInformedSet);
364 return vertices_.size();
372 std::vector<std::shared_ptr<Vertex>>
376 if (vertex->hasCachedNeighbors())
378 return vertex->getNeighbors();
382 ++numNearestNeighborsCalls_;
383 std::vector<std::shared_ptr<Vertex>> neighbors{};
386 vertices_.nearestK(vertex, numNeighbors_, neighbors);
390 vertices_.nearestR(vertex, radius_, neighbors);
392 vertex->cacheNeighbors(neighbors);
399 for (
const auto &start : startVertices_)
401 if (vertex->getId() == start->getId())
411 for (
const auto &goal : goalVertices_)
413 if (vertex->getId() == goal->getId())
423 return startVertices_;
428 return goalVertices_;
433 std::vector<std::shared_ptr<Vertex>> vertices;
434 vertices_.list(vertices);
440 if (!objective_->isFinite(solutionCost_))
445 std::vector<std::shared_ptr<Vertex>> vertices;
446 vertices_.list(vertices);
449 std::vector<std::shared_ptr<Vertex>> verticesToBePruned;
452 for (
const auto &vertex : vertices)
456 if (!canPossiblyImproveSolution(vertex))
463 prunedGoalVertices_.emplace_back(vertex);
467 prunedStartVertices_.emplace_back(vertex);
469 verticesToBePruned.emplace_back(vertex);
474 for (
const auto &vertex : verticesToBePruned)
477 if (vertex->hasReverseParent())
479 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
480 vertex->resetReverseParent();
482 vertex->invalidateReverseBranch();
483 if (vertex->hasForwardParent())
485 vertex->getForwardParent()->removeFromForwardChildren(vertex->getId());
486 vertex->resetForwardParent();
488 vertex->invalidateForwardBranch();
491 vertices_.remove(vertex);
499 return numSampledStates_;
504 return numValidSamples_;
511 return numSampledStates_;
516 return numNearestNeighborsCalls_;
519 double ImplicitGraph::computeConnectionRadius(std::size_t numSamples)
const
522 auto dimension =
static_cast<double>(spaceInformation_->getStateDimension());
525 return rewireFactor_ *
526 std::pow(2.0 * (1.0 + 1.0 / dimension) *
527 (sampler_->getInformedMeasure(solutionCost_) /
529 (std::log(
static_cast<double>(numSamples)) /
static_cast<double>(numSamples)),
541 std::size_t ImplicitGraph::computeNumberOfNeighbors(std::size_t numSamples)
const
543 return std::ceil(rewireFactor_ * k_rgg_ * std::log(
static_cast<double>(numSamples)));
546 bool ImplicitGraph::canPossiblyImproveSolution(
const std::shared_ptr<Vertex> &vertex)
const
549 auto bestCostToCome = objective_->infiniteCost();
550 for (
const auto &start : startVertices_)
552 auto costToCome = objective_->motionCostHeuristic(start->getState(), vertex->getState());
553 if (objective_->isCostBetterThan(costToCome, bestCostToCome))
555 bestCostToCome = costToCome;
561 return objective_->isCostBetterThan(
562 objective_->combineCosts(
563 bestCostToCome, objective_->costToGo(vertex->getState(), problemDefinition_->getGoal().get())),
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
A shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of an abstract state.
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.
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
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.