37#include "ompl/geometric/planners/informedtrees/AITstar.h"
43#include <boost/range/adaptor/reversed.hpp>
45#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
48using namespace std::string_literals;
49using namespace ompl::geometric::aitstar;
56 :
ompl::base::Planner(spaceInformation,
"AITstar")
58 , graph_(solutionCost_)
59 , forwardQueue_([this](const auto &lhs, const auto &rhs) {
return isEdgeBetter(lhs, rhs); })
60 , reverseQueue_([
this](
const auto &lhs,
const auto &rhs) {
return isVertexBetter(lhs, rhs); })
64 specs_.multithreaded =
false;
65 specs_.approximateSolutions =
true;
66 specs_.optimizingPaths =
true;
67 specs_.directed =
true;
68 specs_.provingSolutionNonExistence =
false;
69 specs_.canReportIntermediateSolutions =
true;
84 addPlannerProgressProperty(
"iterations INTEGER", [
this]() {
return std::to_string(numIterations_); });
85 addPlannerProgressProperty(
"best cost DOUBLE", [
this]() {
return std::to_string(solutionCost_.value()); });
86 addPlannerProgressProperty(
"state collision checks INTEGER",
87 [
this]() {
return std::to_string(graph_.getNumberOfStateCollisionChecks()); });
88 addPlannerProgressProperty(
"edge collision checks INTEGER",
89 [
this]() {
return std::to_string(numEdgeCollisionChecks_); });
90 addPlannerProgressProperty(
"nearest neighbour calls INTEGER",
91 [
this]() {
return std::to_string(graph_.getNumberOfNearestNeighborCalls()); });
100 if (
static_cast<bool>(Planner::pdef_))
103 if (!
pdef_->hasOptimizationObjective())
105 OMPL_WARN(
"%s: No optimization objective has been specified. Defaulting to path length.",
106 Planner::getName().c_str());
107 Planner::pdef_->setOptimizationObjective(
108 std::make_shared<ompl::base::PathLengthOptimizationObjective>(Planner::si_));
111 if (
static_cast<bool>(
pdef_->getGoal()))
116 OMPL_ERROR(
"AIT* is currently only implemented for goals that can be cast to "
117 "ompl::base::GOAL_SAMPLEABLE_GOAL_REGION.");
124 objective_ =
pdef_->getOptimizationObjective();
127 solutionCost_ = objective_->infiniteCost();
128 approximateSolutionCost_ = objective_->infiniteCost();
129 approximateSolutionCostToGoal_ = objective_->infiniteCost();
132 motionValidator_ =
si_->getMotionValidator();
141 OMPL_WARN(
"AIT*: Unable to setup without a problem definition.");
179 OMPL_WARN(
"%s: No solution can be found as no start states are available",
name_.c_str());
192 OMPL_WARN(
"%s: No solution can be found as no goal states are available",
name_.c_str());
204 forwardQueue_.
clear();
205 reverseQueue_.
clear();
206 solutionCost_ = objective_->infiniteCost();
207 approximateSolutionCost_ = objective_->infiniteCost();
208 approximateSolutionCostToGoal_ = objective_->infiniteCost();
210 numInconsistentOrUnconnectedTargets_ = 0u;
236 OMPL_INFORM(
"%s: Solving the given planning problem. The current best solution cost is %.4f",
name_.c_str(),
237 solutionCost_.
value());
240 while (!terminationCondition && !objective_->isSatisfied(solutionCost_))
242 iterate(terminationCondition);
247 status = updateSolution();
250 informAboutPlannerStatus(status);
256 return solutionCost_;
263 static std::set<std::shared_ptr<Vertex>,
264 std::function<bool(
const std::shared_ptr<Vertex> &,
const std::shared_ptr<Vertex> &)>>
265 liveStates([](
const auto &lhs,
const auto &rhs) {
return lhs->getId() < rhs->getId(); });
268 Planner::getPlannerData(data);
274 for (
const auto &vertex : vertices)
277 liveStates.insert(vertex);
284 else if (graph_.
isGoal(vertex))
294 if (vertex->hasForwardParent())
298 vertex->getForwardParent()->getId()));
305 batchSize_ = batchSize;
325 trackApproximateSolutions_ = track;
326 if (!trackApproximateSolutions_)
328 if (
static_cast<bool>(objective_))
330 approximateSolutionCost_ = objective_->infiniteCost();
331 approximateSolutionCostToGoal_ = objective_->infiniteCost();
338 return trackApproximateSolutions_;
343 isPruningEnabled_ = prune;
348 return isPruningEnabled_;
371 void AITstar::rebuildForwardQueue()
374 std::vector<Edge> edges;
378 for (
const auto &edge : edges)
380 edge.getChild()->resetForwardQueueIncomingLookup();
381 edge.getParent()->resetForwardQueueOutgoingLookup();
385 forwardQueue_.
clear();
386 numInconsistentOrUnconnectedTargets_ = 0u;
390 for (
auto &edge : edges)
392 insertOrUpdateInForwardQueue(
393 Edge(edge.getParent(), edge.getChild(), computeSortKey(edge.getParent(), edge.getChild())));
397 void AITstar::clearForwardQueue()
399 std::vector<Edge> forwardQueue;
401 for (
const auto &element : forwardQueue)
403 element.getChild()->resetForwardQueueIncomingLookup();
404 element.getParent()->resetForwardQueueOutgoingLookup();
406 forwardQueue_.
clear();
407 numInconsistentOrUnconnectedTargets_ = 0u;
410 void AITstar::rebuildReverseQueue()
413 std::vector<aitstar::KeyVertexPair> content;
415 for (
auto &element : content)
417 element.second->resetReverseQueuePointer();
419 reverseQueue_.
clear();
421 for (
auto &vertex : content)
424 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> element(
425 computeSortKey(vertex.second), vertex.second);
426 auto reverseQueuePointer = reverseQueue_.
insert(element);
427 element.second->setReverseQueuePointer(reverseQueuePointer);
431 void AITstar::clearReverseQueue()
433 std::vector<aitstar::KeyVertexPair> reverseQueue;
435 for (
const auto &element : reverseQueue)
437 element.second->resetReverseQueuePointer();
439 reverseQueue_.
clear();
442 void AITstar::informAboutNewSolution()
const
444 OMPL_INFORM(
"%s (%u iterations): Found a new exact solution of cost %.4f. Sampled a total of %u states, %u "
445 "of which were valid samples (%.1f \%). Processed %u edges, %u of which were collision checked "
446 "(%.1f \%). The forward search tree has %u vertices, %u of which are start states. The reverse "
447 "search tree has %u vertices, %u of which are goal states.",
452 100.0 * (static_cast<double>(graph_.getNumberOfValidSamples()) /
453 static_cast<double>(graph_.getNumberOfSampledStates())),
454 numProcessedEdges_, numEdgeCollisionChecks_,
455 numProcessedEdges_ == 0u ? 0.0 :
456 100.0 * (static_cast<float>(numEdgeCollisionChecks_) /
457 static_cast<float>(numProcessedEdges_)),
458 countNumVerticesInForwardTree(), graph_.getStartVertices().size(),
459 countNumVerticesInReverseTree(), graph_.getGoalVertices().size());
468 OMPL_INFORM(
"%s (%u iterations): Found an exact solution of cost %.4f.",
name_.c_str(),
469 numIterations_, solutionCost_.
value());
474 OMPL_INFORM(
"%s (%u iterations): Did not find an exact solution, but found an approximate "
476 "of cost %.4f which is %.4f away from a goal (in cost space).",
477 name_.c_str(), numIterations_, approximateSolutionCost_.
value(),
478 approximateSolutionCostToGoal_.
value());
483 if (trackApproximateSolutions_)
485 OMPL_INFORM(
"%s (%u iterations): Did not find any solution.",
name_.c_str(), numIterations_);
489 OMPL_INFORM(
"%s (%u iterations): Did not find an exact solution, and tracking approximate "
490 "solutions is disabled.",
491 name_.c_str(), numIterations_);
504 OMPL_INFORM(
"%s (%u iterations): Unable to solve the given planning problem.",
name_.c_str(),
510 "%s (%u iterations): Sampled a total of %u states, %u of which were valid samples (%.1f \%). "
511 "Processed %u edges, %u of which were collision checked (%.1f \%). The forward search tree "
512 "has %u vertices. The reverse search tree has %u vertices.",
516 100.0 * (static_cast<double>(graph_.getNumberOfValidSamples()) /
517 static_cast<double>(graph_.getNumberOfSampledStates())),
518 numProcessedEdges_, numEdgeCollisionChecks_,
519 numProcessedEdges_ == 0u ?
521 100.0 * (static_cast<float>(numEdgeCollisionChecks_) / static_cast<float>(numProcessedEdges_)),
522 countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
525 void AITstar::insertGoalVerticesInReverseQueue()
527 for (
const auto &goal : graph_.getGoalVertices())
530 goal->setExpandedCostToComeFromGoal(objective_->infiniteCost());
531 goal->setCostToComeFromGoal(objective_->identityCost());
534 aitstar::KeyVertexPair element({computeCostToGoToStartHeuristic(goal), objective_->identityCost()},
538 auto reverseQueuePointer = reverseQueue_.
insert(element);
539 goal->setReverseQueuePointer(reverseQueuePointer);
543 void AITstar::expandStartVerticesIntoForwardQueue()
545 for (
const auto &start : graph_.getStartVertices())
547 start->setCostToComeFromStart(objective_->identityCost());
548 insertOrUpdateInForwardQueue(getOutgoingEdges(start));
552 bool AITstar::continueReverseSearch()
const
555 if (reverseQueue_.
empty() || forwardQueue_.
empty())
561 const auto &bestEdge = forwardQueue_.
top()->data;
562 const auto &bestVertex = reverseQueue_.
top()->data;
566 return !((bestEdge.getChild()->isConsistent() &&
567 objective_->isCostBetterThan(bestEdge.getSortKey()[0u], bestVertex.first[0u])) ||
568 numInconsistentOrUnconnectedTargets_ == 0u);
571 bool AITstar::continueForwardSearch()
574 if (forwardQueue_.
empty())
582 const auto &bestEdgeCost = forwardQueue_.
top()->data.getSortKey()[0u];
583 if (!objective_->isFinite(bestEdgeCost))
589 return objective_->isCostBetterThan(bestEdgeCost, solutionCost_);
594 std::vector<Edge> edges;
602 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>>> content;
606 std::vector<std::shared_ptr<Vertex>> vertices;
607 for (
const auto &pair : content)
609 vertices.emplace_back(pair.second);
616 if (!forwardQueue_.
empty())
618 return forwardQueue_.
top()->data;
626 if (!reverseQueue_.
empty())
628 return reverseQueue_.
top()->data.second;
640 vertices.erase(std::remove_if(vertices.begin(), vertices.end(),
641 [
this](
const std::shared_ptr<Vertex> &vertex) {
642 return !graph_.isGoal(vertex) && !vertex->hasReverseParent();
651 if (numIterations_ == 0u)
653 insertGoalVerticesInReverseQueue();
654 expandStartVerticesIntoForwardQueue();
661 if (continueReverseSearch())
663 iterateReverseSearch();
665 else if (continueForwardSearch())
667 iterateForwardSearch();
672 if (graph_.
addSamples(batchSize_, terminationCondition))
675 if (isPruningEnabled_)
681 for (
auto &goal : graph_.getGoalVertices())
683 invalidateCostToComeFromGoalOfReverseBranch(goal);
695 insertGoalVerticesInReverseQueue();
696 expandStartVerticesIntoForwardQueue();
701 void AITstar::iterateForwardSearch()
703 assert(!forwardQueue_.
empty());
706 auto parent = forwardQueue_.
top()->data.getParent();
707 auto child = forwardQueue_.
top()->data.getChild();
708 child->removeFromForwardQueueIncomingLookup(forwardQueue_.
top());
709 parent->removeFromForwardQueueOutgoingLookup(forwardQueue_.
top());
713 assert(child->isConsistent());
714 assert(!graph_.
isGoal(parent));
717 ++numProcessedEdges_;
720 if (child->hasForwardParent() && child->getForwardParent()->getId() == parent->getId())
722 insertOrUpdateInForwardQueue(getOutgoingEdges(child));
725 else if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(),
726 objective_->motionCostHeuristic(
727 parent->getState(), child->getState())),
728 child->getCostToComeFromStart()))
732 if (parent->isWhitelistedAsChild(child) ||
733 motionValidator_->checkMotion(parent->getState(), child->getState()))
736 if (!parent->isWhitelistedAsChild(child))
738 parent->whitelistAsChild(child);
739 numEdgeCollisionChecks_++;
743 const auto edgeCost = objective_->motionCost(parent->getState(), child->getState());
746 if (objective_->isCostBetterThan(
747 objective_->combineCosts(parent->getCostToComeFromStart(), edgeCost),
748 child->getCostToComeFromStart()))
751 child->setForwardParent(parent, edgeCost);
754 parent->addToForwardChildren(child);
757 child->updateCostOfForwardBranch();
760 updateSolution(child);
763 insertOrUpdateInForwardQueue(getOutgoingEdges(child));
769 parent->blacklistAsChild(child);
770 child->blacklistAsChild(parent);
773 if (parent->hasReverseParent() && parent->getReverseParent()->getId() == child->getId())
777 invalidateCostToComeFromGoalOfReverseBranch(parent);
783 void AITstar::iterateReverseSearch()
785 assert(!reverseQueue_.
empty());
788 auto vertex = reverseQueue_.
top()->data.second;
790 vertex->resetReverseQueuePointer();
793 assert(!vertex->isConsistent());
796 if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
799 vertex->setExpandedCostToComeFromGoal(vertex->getCostToComeFromGoal());
800 updateReverseSearchNeighbors(vertex);
803 numInconsistentOrUnconnectedTargets_ -= vertex->getForwardQueueIncomingLookup().size();
808 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
811 updateReverseSearchVertex(vertex);
812 updateReverseSearchNeighbors(vertex);
816 bool AITstar::isEdgeBetter(
const Edge &lhs,
const Edge &rhs)
const
818 return std::lexicographical_compare(
820 [
this](
const auto &a,
const auto &b) { return objective_->isCostBetterThan(a, b); });
823 bool AITstar::isVertexBetter(
const aitstar::KeyVertexPair &lhs,
const aitstar::KeyVertexPair &rhs)
const
827 if (objective_->isCostEquivalentTo(lhs.first[0u], rhs.first[0u]) &&
828 objective_->isCostEquivalentTo(lhs.first[1u], rhs.first[1u]))
830 return !lhs.second->getForwardQueueIncomingLookup().empty() && !lhs.second->isConsistent();
835 return std::lexicographical_compare(
836 lhs.first.cbegin(), lhs.first.cend(), rhs.first.cbegin(), rhs.first.cend(),
837 [
this](
const auto &a,
const auto &b) { return objective_->isCostBetterThan(a, b); });
841 void AITstar::updateReverseSearchVertex(
const std::shared_ptr<Vertex> &vertex)
844 if (graph_.
isGoal(vertex))
846 assert(objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(), objective_->identityCost()));
851 auto bestParent = vertex->getReverseParent();
852 auto bestCost = vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
855 for (
const auto &neighbor : graph_.getNeighbors(vertex))
857 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
858 !vertex->isBlacklistedAsChild(neighbor))
860 auto edgeCost = objective_->motionCostHeuristic(neighbor->getState(), vertex->getState());
861 auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
862 if (objective_->isCostBetterThan(parentCost,
bestCost))
864 bestParent = neighbor;
871 for (
const auto &forwardChild : vertex->getForwardChildren())
873 auto edgeCost = objective_->motionCostHeuristic(forwardChild->getState(), vertex->getState());
874 auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
876 if (objective_->isCostBetterThan(parentCost,
bestCost))
878 bestParent = forwardChild;
884 if (vertex->hasForwardParent())
886 auto forwardParent = vertex->getForwardParent();
887 auto edgeCost = objective_->motionCostHeuristic(forwardParent->getState(), vertex->getState());
888 auto parentCost = objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
890 if (objective_->isCostBetterThan(parentCost,
bestCost))
892 bestParent = forwardParent;
898 vertex->setCostToComeFromGoal(
bestCost);
904 vertex->setReverseParent(bestParent);
905 bestParent->addToReverseChildren(vertex);
910 if (vertex->hasReverseParent())
912 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
913 vertex->resetReverseParent();
918 if (!vertex->isConsistent())
920 insertOrUpdateInReverseQueue(vertex);
924 auto reverseQueuePointer = vertex->getReverseQueuePointer();
925 if (reverseQueuePointer)
927 reverseQueue_.
remove(reverseQueuePointer);
928 vertex->resetReverseQueuePointer();
935 for (
const auto &element : vertex->getForwardQueueIncomingLookup())
937 auto &edge = element->data;
938 edge.setSortKey(computeSortKey(edge.getParent(), edge.getChild()));
939 forwardQueue_.
update(element);
943 void AITstar::updateReverseSearchNeighbors(
const std::shared_ptr<Vertex> &vertex)
947 for (
const auto &child : vertex->getReverseChildren())
949 updateReverseSearchVertex(child);
953 for (
const auto &neighbor : graph_.getNeighbors(vertex))
955 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
956 !vertex->isBlacklistedAsChild(neighbor))
958 updateReverseSearchVertex(neighbor);
963 for (
const auto &child : vertex->getForwardChildren())
965 updateReverseSearchVertex(child);
969 if (vertex->hasForwardParent())
971 updateReverseSearchVertex(vertex->getForwardParent());
975 void AITstar::insertOrUpdateInReverseQueue(
const std::shared_ptr<Vertex> &vertex)
978 auto element = vertex->getReverseQueuePointer();
983 element->data.first = computeSortKey(vertex);
984 reverseQueue_.
update(element);
989 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> element(computeSortKey(vertex),
993 auto reverseQueuePointer = reverseQueue_.
insert(element);
994 vertex->setReverseQueuePointer(reverseQueuePointer);
998 void AITstar::insertOrUpdateInForwardQueue(
const Edge &edge)
1001 const auto lookup = edge.
getChild()->getForwardQueueIncomingLookup();
1002 const auto it = std::find_if(lookup.begin(), lookup.end(), [&edge](
const auto element) {
1003 return element->data.getParent()->getId() == edge.getParent()->getId();
1006 if (it != lookup.end())
1010 assert(std::find_if(edge.
getParent()->getForwardQueueOutgoingLookup().begin(),
1011 edge.
getParent()->getForwardQueueOutgoingLookup().end(),
1012 [&edge](
const auto element) {
1013 return element->data.getChild()->getId() == edge.getChild()->getId();
1014 }) != edge.
getParent()->getForwardQueueOutgoingLookup().end());
1017 if (isEdgeBetter(edge, (*it)->data))
1020 forwardQueue_.
update(*it);
1025 auto element = forwardQueue_.
insert(edge);
1026 edge.
getParent()->addToForwardQueueOutgoingLookup(element);
1027 edge.
getChild()->addToForwardQueueIncomingLookup(element);
1030 if (!edge.
getChild()->isConsistent() || !objective_->isFinite(edge.
getChild()->getCostToComeFromGoal()))
1032 ++numInconsistentOrUnconnectedTargets_;
1037 void AITstar::insertOrUpdateInForwardQueue(
const std::vector<Edge> &edges)
1039 for (
const auto &edge : edges)
1041 insertOrUpdateInForwardQueue(edge);
1045 std::shared_ptr<ompl::geometric::PathGeometric>
1046 AITstar::getPathToVertex(
const std::shared_ptr<Vertex> &vertex)
const
1049 std::vector<std::shared_ptr<Vertex>> reversePath;
1050 auto current = vertex;
1051 while (!graph_.
isStart(current))
1053 reversePath.emplace_back(current);
1054 current = current->getForwardParent();
1056 reversePath.emplace_back(current);
1059 auto path = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
1060 for (
const auto &vertex : boost::adaptors::reverse(reversePath))
1062 path->append(vertex->getState());
1068 std::array<ompl::base::Cost, 3u> AITstar::computeSortKey(
const std::shared_ptr<Vertex> &parent,
1069 const std::shared_ptr<Vertex> &child)
const
1073 ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1075 objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1076 child->getCostToGoToGoal()),
1077 objective_->combineCosts(edgeCostHeuristic, child->getCostToGoToGoal()),
1078 parent->getCostToComeFromStart()};
1081 std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(
const std::shared_ptr<Vertex> &vertex)
const
1084 return {objective_->combineCosts(objective_->betterCost(vertex->getCostToComeFromGoal(),
1085 vertex->getExpandedCostToComeFromGoal()),
1086 computeCostToGoToStartHeuristic(vertex)),
1087 objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1090 std::vector<Edge> AITstar::getOutgoingEdges(
const std::shared_ptr<Vertex> &vertex)
const
1093 std::vector<Edge> outgoingEdges;
1096 for (
const auto &child : vertex->getForwardChildren())
1098 outgoingEdges.emplace_back(vertex, child, computeSortKey(vertex, child));
1102 for (
const auto &neighbor : graph_.getNeighbors(vertex))
1105 if (vertex->getId() == neighbor->getId())
1111 if (vertex->hasReverseParent() && neighbor->getId() == vertex->getReverseParent()->getId())
1117 if (neighbor->isBlacklistedAsChild(vertex) || vertex->isBlacklistedAsChild(neighbor))
1123 outgoingEdges.emplace_back(vertex, neighbor, computeSortKey(vertex, neighbor));
1127 if (vertex->hasReverseParent())
1129 const auto &reverseParent = vertex->getReverseParent();
1130 outgoingEdges.emplace_back(vertex, reverseParent, computeSortKey(vertex, reverseParent));
1133 return outgoingEdges;
1136 void AITstar::updateExactSolution()
1139 for (
const auto &goal : graph_.getGoalVertices())
1143 if (objective_->isCostBetterThan(goal->getCostToComeFromStart(), solutionCost_) ||
1144 (!
pdef_->hasExactSolution() && objective_->isFinite(goal->getCostToComeFromStart())))
1147 solutionCost_ = goal->getCostToComeFromStart();
1151 solution.setPlannerName(
name_);
1154 solution.setOptimized(objective_, solutionCost_, objective_->isSatisfied(solutionCost_));
1157 pdef_->addSolutionPath(solution);
1160 informAboutNewSolution();
1165 void AITstar::updateApproximateSolution()
1167 for (
auto &start : graph_.getStartVertices())
1169 start->callOnForwardBranch([
this](
const auto &vertex) ->
void { updateApproximateSolution(vertex); });
1173 void AITstar::updateApproximateSolution(
const std::shared_ptr<Vertex> &vertex)
1175 assert(trackApproximateSolutions_);
1176 if (vertex->hasForwardParent() || graph_.
isStart(vertex))
1178 auto costToGoal = computeCostToGoToGoal(vertex);
1182 if (objective_->isCostBetterThan(costToGoal, approximateSolutionCostToGoal_) ||
1183 !
pdef_->hasApproximateSolution())
1186 approximateSolutionCost_ = vertex->getCostToComeFromStart();
1187 approximateSolutionCostToGoal_ = costToGoal;
1189 solution.setPlannerName(
name_);
1192 solution.setApproximate(costToGoal.value());
1195 solution.setOptimized(objective_, approximateSolutionCost_,
false);
1198 pdef_->addSolutionPath(solution);
1205 updateExactSolution();
1206 if (objective_->isFinite(solutionCost_))
1210 else if (trackApproximateSolutions_)
1212 updateApproximateSolution();
1223 updateExactSolution();
1224 if (objective_->isFinite(solutionCost_))
1228 else if (trackApproximateSolutions_)
1230 updateApproximateSolution(vertex);
1239 ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(
const std::shared_ptr<Vertex> &vertex)
const
1243 for (
const auto &start : graph_.getStartVertices())
1246 bestCost, objective_->motionCostHeuristic(vertex->getState(), start->getState()));
1251 ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(
const std::shared_ptr<Vertex> &vertex)
const
1255 for (
const auto &goal : graph_.getGoalVertices())
1258 bestCost, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
1263 ompl::base::Cost AITstar::computeCostToGoToGoal(
const std::shared_ptr<Vertex> &vertex)
const
1267 for (
const auto &goal : graph_.getGoalVertices())
1270 objective_->betterCost(
bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1279 for (
const auto &start : graph_.getStartVertices())
1281 bestCost = objective_->betterCost(
bestCost, start->getCostToComeFromGoal());
1286 std::size_t AITstar::countNumVerticesInForwardTree()
const
1288 std::size_t numVerticesInForwardTree = 0u;
1290 for (
const auto &vertex : vertices)
1292 if (graph_.
isStart(vertex) || vertex->hasForwardParent())
1294 ++numVerticesInForwardTree;
1297 return numVerticesInForwardTree;
1300 std::size_t AITstar::countNumVerticesInReverseTree()
const
1302 std::size_t numVerticesInReverseTree = 0u;
1304 for (
const auto &vertex : vertices)
1306 if (graph_.
isGoal(vertex) || vertex->hasReverseParent())
1308 ++numVerticesInReverseTree;
1311 return numVerticesInReverseTree;
1314 void AITstar::invalidateCostToComeFromGoalOfReverseBranch(
const std::shared_ptr<Vertex> &vertex)
1318 if (vertex->isConsistent())
1320 numInconsistentOrUnconnectedTargets_ += vertex->getForwardQueueIncomingLookup().size();
1324 if (!graph_.
isGoal(vertex))
1327 vertex->resetCostToComeFromGoal();
1330 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
1331 vertex->resetReverseParent();
1335 vertex->resetExpandedCostToComeFromGoal();
1338 for (
const auto &edge : vertex->getForwardQueueIncomingLookup())
1341 forwardQueue_.
update(edge);
1345 auto reverseQueuePointer = vertex->getReverseQueuePointer();
1346 if (reverseQueuePointer)
1348 reverseQueue_.
remove(reverseQueuePointer);
1349 vertex->resetReverseQueuePointer();
1353 for (
const auto &child : vertex->getReverseChildren())
1355 invalidateCostToComeFromGoalOfReverseBranch(child);
1359 updateReverseSearchVertex(vertex);
Element * insert(const _T &data)
Add a new element.
void pop()
Remove the top element.
bool empty() const
Check if the heap is empty.
void remove(Element *element)
Remove a specific element.
void update(Element *element)
Update an element in the heap.
void getContent(std::vector< _T > &content) const
Get the data stored in this heap.
Element * top() const
Return the top element. nullptr for an empty heap.
void clear()
Clear the heap.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
double value() const
The value of the cost.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
PlannerInputStates pis_
Utility class to extract valid input states
ProblemDefinitionPtr pdef_
The user set problem definition.
std::string name_
The name of this planner.
SpaceInformationPtr si_
The space information for which planning is done.
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
bool setup_
Flag indicating whether setup() has been called.
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
void enablePruning(bool prune)
Set whether pruning is enabled or not.
void setMaxNumberOfGoals(unsigned int numberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
double getRewireFactor() const
Get the rewire factor of the RGG graph.
bool isPruningEnabled() const
Get whether pruning is enabled or not.
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
void setBatchSize(std::size_t batchSize)
Set the batch size.
ompl::base::PlannerStatus::StatusType ensureStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition)
Checks whether the problem is successfully setup.
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
Constructs a AIT*.
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
void setup() override
Additional setup that can only be done once a problem definition is set.
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
ompl::base::PlannerStatus::StatusType ensureSetup()
Checks whether the planner is successfully setup.
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
void setUseKNearest(bool useKNearest)
Set whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
void clear() override
Clears the algorithm's internal state.
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
std::size_t getBatchSize() const
Get the batch size.
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
std::shared_ptr< Vertex > getChild() const
Returns the child in this edge.
const std::array< ompl::base::Cost, 3u > & getSortKey() const
Returns the sort key associated with this edge.
void setSortKey(const std::array< ompl::base::Cost, 3u > &key)
Sets the sort key associated with this edge.
std::shared_ptr< Vertex > getParent() const
Returns the parent in this edge.
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::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
void setMaxNumberOfGoals(unsigned int maxNumberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
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 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.
void clear()
Resets the graph to its construction state, without resetting options.
bool addSamples(std::size_t numNewSamples, const ompl::base::PlannerTerminationCondition &terminationCondition)
Adds a batch of samples and returns the samples it has added.
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.
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.
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#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.
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Main namespace. Contains everything in this library.
Representation of a solution to a planning problem.
A class to store the exit status of Planner::solve()
StatusType
The possible values of the status returned by a planner.
@ ABORT
The planner did not find a solution for some other reason.
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
@ TIMEOUT
The planner failed to find a solution.
@ UNKNOWN
Uninitialized status.
@ TYPE_COUNT
The number of possible status values.
@ INFEASIBLE
The planner decided that problem is infeasible.
@ CRASH
The planner crashed.