41 #include <boost/range/adaptor/reversed.hpp>
43 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
44 #include "ompl/geometric/planners/informedtrees/AITstar.h"
47 using namespace std::string_literals;
54 :
ompl::base::Planner(spaceInformation,
"AITstar")
56 , graph_(solutionCost_)
57 , forwardQueue_([this](const aitstar::Edge &lhs, const aitstar::Edge &rhs) {
58 return std::lexicographical_compare(lhs.getSortKey().cbegin(), lhs.getSortKey().cend(),
59 rhs.getSortKey().cbegin(), rhs.getSortKey().cend(),
61 return objective_->isCostBetterThan(a, b);
65 [
this](
const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> &lhs,
66 const std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> &rhs) {
67 return std::lexicographical_compare(lhs.first.cbegin(), lhs.first.cend(), rhs.first.cbegin(),
70 return objective_->isCostBetterThan(a, b);
76 specs_.multithreaded =
false;
77 specs_.approximateSolutions =
true;
78 specs_.optimizingPaths =
true;
79 specs_.directed =
true;
80 specs_.provingSolutionNonExistence =
false;
81 specs_.canReportIntermediateSolutions =
true;
94 addPlannerProgressProperty(
"iterations INTEGER", [
this]() {
return std::to_string(numIterations_); });
95 addPlannerProgressProperty(
"best cost DOUBLE", [
this]() {
return std::to_string(solutionCost_.value()); });
96 addPlannerProgressProperty(
"state collision checks INTEGER",
97 [
this]() {
return std::to_string(graph_.getNumberOfStateCollisionChecks()); });
98 addPlannerProgressProperty(
"edge collision checks INTEGER",
99 [
this]() {
return std::to_string(numEdgeCollisionChecks_); });
100 addPlannerProgressProperty(
"nearest neighbour calls INTEGER",
101 [
this]() {
return std::to_string(graph_.getNumberOfNearestNeighborCalls()); });
110 if (
static_cast<bool>(Planner::pdef_))
113 if (!
pdef_->hasOptimizationObjective())
115 OMPL_WARN(
"%s: No optimization objective has been specified. Defaulting to path length.",
116 Planner::getName().c_str());
117 Planner::pdef_->setOptimizationObjective(
118 std::make_shared<ompl::base::PathLengthOptimizationObjective>(Planner::si_));
121 if (
static_cast<bool>(
pdef_->getGoal()))
126 OMPL_ERROR(
"AIT* is currently only implemented for goals that can be cast to "
127 "ompl::base::GOAL_SAMPLEABLE_GOAL_REGION.");
134 objective_ =
pdef_->getOptimizationObjective();
137 solutionCost_ = objective_->infiniteCost();
138 approximateSolutionCost_ = objective_->infiniteCost();
139 approximateSolutionCostToGoal_ = objective_->infiniteCost();
142 motionValidator_ =
si_->getMotionValidator();
151 OMPL_WARN(
"AIT*: Unable to setup without a problem definition.");
158 forwardQueue_.
clear();
159 reverseQueue_.
clear();
160 solutionCost_ = objective_->infiniteCost();
161 approximateSolutionCost_ = objective_->infiniteCost();
162 approximateSolutionCostToGoal_ = objective_->infiniteCost();
163 edgesToBeInserted_.clear();
165 performReverseSearchIteration_ =
true;
166 isForwardSearchStartedOnBatch_ =
false;
167 forwardQueueMustBeRebuilt_ =
false;
174 auto status = ompl::base::PlannerStatus::StatusType::UNKNOWN;
177 Planner::checkValidity();
178 if (!Planner::setup_)
180 OMPL_WARN(
"%s: Failed to setup and thus solve can not do anything meaningful.",
name_.c_str());
181 status = ompl::base::PlannerStatus::StatusType::ABORT;
182 informAboutPlannerStatus(status);
194 OMPL_WARN(
"%s: No solution can be found as no start states are available",
name_.c_str());
195 status = ompl::base::PlannerStatus::StatusType::INVALID_START;
196 informAboutPlannerStatus(status);
203 OMPL_WARN(
"%s: No solution can be found as no goal states are available",
name_.c_str());
204 status = ompl::base::PlannerStatus::StatusType::INVALID_GOAL;
205 informAboutPlannerStatus(status);
209 OMPL_INFORM(
"%s: Searching for a solution to the given planning problem. The current best solution cost is "
214 while (!terminationCondition && !objective_->isSatisfied(solutionCost_))
221 updateExactSolution();
225 if (!
pdef_->hasExactSolution() && trackApproximateSolutions_)
229 updateApproximateSolution(vertex);
234 if (objective_->isFinite(solutionCost_))
236 status = ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION;
238 else if (trackApproximateSolutions_ && objective_->isFinite(approximateSolutionCost_))
240 status = ompl::base::PlannerStatus::StatusType::APPROXIMATE_SOLUTION;
244 status = ompl::base::PlannerStatus::StatusType::TIMEOUT;
247 informAboutPlannerStatus(status);
253 return solutionCost_;
261 std::shared_ptr<aitstar::Vertex>,
262 std::function<bool(
const std::shared_ptr<aitstar::Vertex> &,
const std::shared_ptr<aitstar::Vertex> &)>>
263 liveStates([](
const auto &lhs,
const auto &rhs) {
return lhs->getId() < rhs->getId(); });
266 Planner::getPlannerData(data);
272 for (
const auto &vertex : vertices)
275 liveStates.insert(vertex);
282 else if (graph_.
isGoal(vertex))
292 if (vertex->hasForwardParent())
296 vertex->getForwardParent()->getId()));
303 batchSize_ = batchSize;
323 trackApproximateSolutions_ = track;
324 if (!trackApproximateSolutions_)
326 if (
static_cast<bool>(objective_))
328 approximateSolutionCost_ = objective_->infiniteCost();
329 approximateSolutionCostToGoal_ = objective_->infiniteCost();
336 return trackApproximateSolutions_;
341 isPruningEnabled_ = prune;
346 return isPruningEnabled_;
361 repairReverseSearch_ = repairReverseSearch;
364 void AITstar::rebuildForwardQueue()
367 std::vector<aitstar::Edge> edges;
371 for (
const auto &edge : edges)
373 edge.getChild()->resetForwardQueueIncomingLookup();
374 edge.getParent()->resetForwardQueueOutgoingLookup();
378 forwardQueue_.
clear();
382 if (haveAllVerticesBeenProcessed(edges))
384 for (
auto &edge : edges)
386 insertOrUpdateInForwardQueue(aitstar::Edge(edge.getParent(), edge.getChild(),
387 computeSortKey(edge.getParent(), edge.getChild())));
392 edgesToBeInserted_ = edges;
393 performReverseSearchIteration_ =
true;
397 void AITstar::rebuildReverseQueue()
400 std::vector<KeyVertexPair> content;
402 for (
auto &element : content)
404 element.second->resetReverseQueuePointer();
406 reverseQueue_.
clear();
408 for (
auto &vertex : content)
411 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
412 computeSortKey(vertex.second), vertex.second);
413 auto reverseQueuePointer = reverseQueue_.
insert(element);
414 element.second->setReverseQueuePointer(reverseQueuePointer);
418 void AITstar::informAboutNewSolution()
const
420 OMPL_INFORM(
"%s (%u iterations): Found a new exact solution of cost %.4f. Sampled a total of %u states, %u "
421 "of which were valid samples (%.1f \%). Processed %u edges, %u of which were collision checked "
422 "(%.1f \%). The forward search tree has %u vertices. The reverse search tree has %u vertices.",
429 numProcessedEdges_, numEdgeCollisionChecks_,
430 numProcessedEdges_ == 0u ? 0.0 :
431 100.0 * (
static_cast<float>(numEdgeCollisionChecks_) /
432 static_cast<float>(numProcessedEdges_)),
433 countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
440 case ompl::base::PlannerStatus::StatusType::EXACT_SOLUTION:
442 OMPL_INFORM(
"%s (%u iterations): Found an exact solution of cost %.4f.",
name_.c_str(),
443 numIterations_, solutionCost_.
value());
446 case ompl::base::PlannerStatus::StatusType::APPROXIMATE_SOLUTION:
448 OMPL_INFORM(
"%s (%u iterations): Did not find an exact solution, but found an approximate solution "
449 "of cost %.4f which is %.4f away from a goal (in cost space).",
450 name_.c_str(), numIterations_, approximateSolutionCost_.
value(),
451 approximateSolutionCostToGoal_.
value());
454 case ompl::base::PlannerStatus::StatusType::TIMEOUT:
456 if (trackApproximateSolutions_)
458 OMPL_INFORM(
"%s (%u iterations): Did not find any solution.",
name_.c_str(), numIterations_);
462 OMPL_INFORM(
"%s (%u iterations): Did not find an exact solution, and tracking approximate "
463 "solutions is disabled.",
464 name_.c_str(), numIterations_);
468 case ompl::base::PlannerStatus::StatusType::UNKNOWN:
469 case ompl::base::PlannerStatus::StatusType::INVALID_START:
470 case ompl::base::PlannerStatus::StatusType::INVALID_GOAL:
471 case ompl::base::PlannerStatus::StatusType::UNRECOGNIZED_GOAL_TYPE:
472 case ompl::base::PlannerStatus::StatusType::CRASH:
473 case ompl::base::PlannerStatus::StatusType::ABORT:
474 case ompl::base::PlannerStatus::StatusType::TYPE_COUNT:
476 OMPL_INFORM(
"%s (%u iterations): Unable to solve the given planning problem.",
name_.c_str(),
482 "%s (%u iterations): Sampled a total of %u states, %u of which were valid samples (%.1f \%). "
483 "Processed %u edges, %u of which were collision checked (%.1f \%). The forward search tree "
484 "has %u vertices. The reverse search tree has %u vertices.",
490 numProcessedEdges_, numEdgeCollisionChecks_,
491 numProcessedEdges_ == 0u ?
493 100.0 * (
static_cast<float>(numEdgeCollisionChecks_) /
static_cast<float>(numProcessedEdges_)),
494 countNumVerticesInForwardTree(), countNumVerticesInReverseTree());
499 std::vector<aitstar::Edge> edges;
507 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>> content;
511 std::vector<std::shared_ptr<aitstar::Vertex>> vertices;
512 for (
const auto &pair : content)
514 vertices.emplace_back(pair.second);
521 if (!forwardQueue_.
empty())
523 return forwardQueue_.
top()->data;
531 if (!reverseQueue_.
empty())
533 return reverseQueue_.
top()->data.second;
545 vertices.erase(std::remove_if(vertices.begin(), vertices.end(),
546 [
this](
const std::shared_ptr<aitstar::Vertex> &vertex) {
547 return !graph_.isGoal(vertex) && !vertex->hasReverseParent();
553 void AITstar::iterate()
556 if (numIterations_ == 0u)
561 goal->setCostToComeFromGoal(objective_->identityCost());
564 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
565 std::array<ompl::base::Cost, 2u>(
570 auto reverseQueuePointer = reverseQueue_.
insert(element);
571 goal->setReverseQueuePointer(reverseQueuePointer);
579 if (performReverseSearchIteration_)
582 if (!reverseQueue_.
empty())
584 performReverseSearchIteration();
593 for (
const auto &edge : edgesToBeInserted_)
595 if (haveAllVerticesBeenProcessed(edge))
597 insertOrUpdateInForwardQueue(aitstar::Edge(
598 edge.getParent(), edge.getChild(), computeSortKey(edge.getParent(), edge.getChild())));
601 edgesToBeInserted_.clear();
602 performReverseSearchIteration_ =
false;
603 forwardQueueMustBeRebuilt_ =
true;
608 if (!isForwardSearchStartedOnBatch_)
611 isForwardSearchStartedOnBatch_ =
true;
615 std::vector<aitstar::Edge> outgoingStartEdges;
618 if (objective_->isFinite(start->getCostToComeFromGoal()))
623 const auto outgoingEdges = getOutgoingEdges(start);
624 outgoingStartEdges.insert(outgoingStartEdges.end(), outgoingEdges.begin(),
625 outgoingEdges.end());
631 if (haveAllVerticesBeenProcessed(outgoingStartEdges))
633 for (
const auto &edge : outgoingStartEdges)
635 insertOrUpdateInForwardQueue(edge);
640 assert(edgesToBeInserted_.empty());
641 edgesToBeInserted_ = outgoingStartEdges;
642 performReverseSearchIteration_ =
true;
645 else if (forwardQueueMustBeRebuilt_)
648 rebuildForwardQueue();
649 forwardQueueMustBeRebuilt_ =
false;
651 else if (!forwardQueue_.
empty())
654 performForwardSearchIteration();
660 std::vector<std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>>>
663 for (
const auto &element : reverseQueue)
665 element.second->resetReverseQueuePointer();
667 reverseQueue_.
clear();
670 std::vector<aitstar::Edge> forwardQueue;
672 for (
const auto &element : forwardQueue)
674 element.getChild()->resetForwardQueueIncomingLookup();
675 element.getParent()->resetForwardQueueOutgoingLookup();
677 forwardQueue_.
clear();
680 edgesToBeInserted_.clear();
689 if (isPruningEnabled_)
700 auto reverseQueuePointer = reverseQueue_.
insert(std::make_pair(computeSortKey(goal), goal));
701 goal->setReverseQueuePointer(reverseQueuePointer);
702 goal->setCostToComeFromGoal(objective_->identityCost());
706 isForwardSearchStartedOnBatch_ =
false;
709 performReverseSearchIteration_ =
true;
714 void AITstar::performForwardSearchIteration()
717 assert(edgesToBeInserted_.empty());
720 auto &edge = forwardQueue_.
top()->data;
721 auto parent = edge.getParent();
722 auto child = edge.getChild();
725 assert(child->hasReverseParent() || graph_.
isGoal(child));
726 assert(parent->hasReverseParent() || graph_.
isGoal(parent));
729 child->removeFromForwardQueueIncomingLookup(forwardQueue_.
top());
730 parent->removeFromForwardQueueOutgoingLookup(forwardQueue_.
top());
736 ++numProcessedEdges_;
740 parent->registerPoppedOutgoingEdgeDuringForwardSearch();
743 auto edgeCost = objective_->motionCostHeuristic(parent->getState(), child->getState());
744 auto parentCostToGoToGoal = objective_->combineCosts(edgeCost, child->getCostToGoToGoal());
745 auto pathThroughEdgeCost = objective_->combineCosts(parent->getCostToComeFromStart(), parentCostToGoToGoal);
746 if (!objective_->isCostBetterThan(pathThroughEdgeCost, solutionCost_))
748 if (objective_->isFinite(pathThroughEdgeCost) ||
749 !objective_->isFinite(computeBestCostToComeFromGoalOfAnyStart()))
751 std::vector<aitstar::Edge> edges;
753 for (
const auto &edge : edges)
755 edge.getChild()->resetForwardQueueIncomingLookup();
756 edge.getParent()->resetForwardQueueOutgoingLookup();
758 forwardQueue_.
clear();
762 performReverseSearchIteration_ =
true;
765 else if (child->hasForwardParent() && child->getForwardParent()->getId() == parent->getId())
768 auto edges = getOutgoingEdges(child);
769 if (haveAllVerticesBeenProcessed(edges))
771 for (
const auto &edge : edges)
773 insertOrUpdateInForwardQueue(edge);
778 edgesToBeInserted_ = edges;
779 performReverseSearchIteration_ =
true;
783 else if (objective_->isCostBetterThan(child->getCostToComeFromStart(),
784 objective_->combineCosts(parent->getCostToComeFromStart(),
785 objective_->motionCostHeuristic(
786 parent->getState(), child->getState()))))
791 else if (parent->isWhitelistedAsChild(child) ||
792 motionValidator_->checkMotion(parent->getState(), child->getState()))
795 if (!parent->isWhitelistedAsChild(child))
797 parent->whitelistAsChild(child);
798 numEdgeCollisionChecks_++;
802 auto edgeCost = objective_->motionCost(parent->getState(), child->getState());
805 if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCost),
806 child->getCostToComeFromStart()))
809 assert(!child->hasHadOutgoingEdgePoppedDuringCurrentForwardSearch());
812 child->setForwardParent(parent, edgeCost);
815 parent->addToForwardChildren(child);
818 child->updateCostOfForwardBranch();
821 updateExactSolution();
825 if (!
pdef_->hasExactSolution() && trackApproximateSolutions_)
827 updateApproximateSolution(child);
831 auto edges = getOutgoingEdges(child);
832 if (haveAllVerticesBeenProcessed(edges))
834 for (
const auto &edge : edges)
836 insertOrUpdateInForwardQueue(edge);
841 edgesToBeInserted_ = edges;
842 performReverseSearchIteration_ =
true;
850 parent->blacklistAsChild(child);
853 if (repairReverseSearch_)
855 if (parent->hasReverseParent() && parent->getReverseParent()->getId() == child->getId())
858 parent->setCostToComeFromGoal(objective_->infiniteCost());
859 parent->setExpandedCostToComeFromGoal(objective_->infiniteCost());
860 parent->resetReverseParent();
861 child->removeFromReverseChildren(parent->getId());
864 invalidateCostToComeFromGoalOfReverseBranch(parent);
867 rebuildReverseQueue();
870 reverseSearchUpdateVertex(parent);
873 if (reverseQueue_.
empty())
875 std::vector<aitstar::Edge> edges;
877 for (
const auto &edge : edges)
879 edge.getChild()->resetForwardQueueIncomingLookup();
880 edge.getParent()->resetForwardQueueOutgoingLookup();
882 forwardQueue_.
clear();
886 performReverseSearchIteration_ =
true;
893 void AITstar::performReverseSearchIteration()
895 assert(!reverseQueue_.
empty());
898 auto vertex = reverseQueue_.
top()->data.second;
902 vertex->resetReverseQueuePointer();
905 assert((!objective_->isFinite(vertex->getCostToComeFromGoal()) &&
906 !objective_->isFinite(vertex->getExpandedCostToComeFromGoal())) ||
907 (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
908 vertex->getExpandedCostToComeFromGoal())));
911 bool underconsistentStart{
false};
914 if (objective_->isCostBetterThan(start->getExpandedCostToComeFromGoal(),
915 start->getCostToComeFromGoal()))
917 underconsistentStart =
true;
923 if (edgesToBeInserted_.empty() &&
924 ((!underconsistentStart &&
925 !objective_->isCostBetterThan(objective_->combineCosts(vertex->getCostToComeFromGoal(),
926 computeCostToGoToStartHeuristic(vertex)),
928 objective_->isCostBetterThan(
929 ompl::base::Cost(computeBestCostToComeFromGoalOfAnyStart().value() + 1e-6), solutionCost_)))
932 performReverseSearchIteration_ =
false;
933 forwardQueueMustBeRebuilt_ =
true;
934 vertex->registerExpansionDuringReverseSearch();
939 if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
942 vertex->registerExpansionDuringReverseSearch();
947 vertex->registerExpansionDuringReverseSearch();
948 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
949 reverseSearchUpdateVertex(vertex);
955 for (
const auto &child : vertex->getReverseChildren())
957 reverseSearchUpdateVertex(child);
961 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
963 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
964 !vertex->isBlacklistedAsChild(neighbor))
966 reverseSearchUpdateVertex(neighbor);
971 for (
const auto &child : vertex->getForwardChildren())
973 reverseSearchUpdateVertex(child);
977 if (vertex->hasForwardParent())
979 reverseSearchUpdateVertex(vertex->getForwardParent());
982 if (!edgesToBeInserted_.empty())
984 if (haveAllVerticesBeenProcessed(edgesToBeInserted_))
986 for (std::size_t i = 0u; i < edgesToBeInserted_.size(); ++i)
988 auto &edge = edgesToBeInserted_.at(i);
989 edge.setSortKey(computeSortKey(edge.getParent(), edge.getChild()));
990 insertOrUpdateInForwardQueue(edge);
992 edgesToBeInserted_.clear();
997 void AITstar::reverseSearchUpdateVertex(
const std::shared_ptr<aitstar::Vertex> &vertex)
999 if (!graph_.
isGoal(vertex))
1002 auto bestParent = vertex->getReverseParent();
1004 vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
1007 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
1009 if (neighbor->getId() != vertex->getId() && !neighbor->isBlacklistedAsChild(vertex) &&
1010 !vertex->isBlacklistedAsChild(neighbor))
1012 auto edgeCost = objective_->motionCostHeuristic(neighbor->getState(), vertex->getState());
1013 auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
1014 if (objective_->isCostBetterThan(parentCost,
bestCost))
1016 bestParent = neighbor;
1023 for (
const auto &forwardChild : vertex->getForwardChildren())
1025 auto edgeCost = objective_->motionCostHeuristic(forwardChild->getState(), vertex->getState());
1026 auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
1028 if (objective_->isCostBetterThan(parentCost,
bestCost))
1030 bestParent = forwardChild;
1036 if (vertex->hasForwardParent())
1038 auto forwardParent = vertex->getForwardParent();
1039 auto edgeCost = objective_->motionCostHeuristic(forwardParent->getState(), vertex->getState());
1041 objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
1043 if (objective_->isCostBetterThan(parentCost,
bestCost))
1045 bestParent = forwardParent;
1051 if (vertex->hasReverseParent())
1053 auto reverseParent = vertex->getReverseParent();
1054 auto edgeCost = objective_->motionCostHeuristic(reverseParent->getState(), vertex->getState());
1056 objective_->combineCosts(reverseParent->getExpandedCostToComeFromGoal(), edgeCost);
1058 if (objective_->isCostBetterThan(parentCost,
bestCost))
1060 bestParent = reverseParent;
1066 if (!objective_->isFinite(
bestCost))
1069 if (vertex->hasReverseParent())
1071 vertex->getReverseParent()->removeFromReverseChildren(vertex->getId());
1072 vertex->resetReverseParent();
1076 vertex->setCostToComeFromGoal(objective_->infiniteCost());
1077 vertex->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1078 auto affectedVertices = vertex->invalidateReverseBranch();
1081 for (
const auto &affectedVertex : affectedVertices)
1083 auto forwardQueueIncomingLookup = affectedVertex.lock()->getForwardQueueIncomingLookup();
1084 for (
const auto &element : forwardQueueIncomingLookup)
1086 edgesToBeInserted_.emplace_back(element->data);
1087 element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1088 forwardQueue_.
remove(element);
1090 affectedVertex.lock()->resetForwardQueueIncomingLookup();
1092 auto forwardQueueOutgoingLookup = affectedVertex.lock()->getForwardQueueOutgoingLookup();
1093 for (
const auto &element : forwardQueueOutgoingLookup)
1095 edgesToBeInserted_.emplace_back(element->data);
1096 element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1097 forwardQueue_.
remove(element);
1099 affectedVertex.lock()->resetForwardQueueOutgoingLookup();
1103 auto vertexForwardQueueIncomingLookup = vertex->getForwardQueueIncomingLookup();
1104 for (
const auto &element : vertexForwardQueueIncomingLookup)
1106 auto &edge = element->data;
1107 auto it = std::find_if(affectedVertices.begin(), affectedVertices.end(),
1108 [edge](
const auto &affectedVertex) {
1109 return affectedVertex.lock()->getId() == edge.getParent()->getId();
1111 if (it != affectedVertices.end())
1113 edgesToBeInserted_.emplace_back(element->data);
1114 vertex->removeFromForwardQueueIncomingLookup(element);
1115 element->data.getParent()->removeFromForwardQueueOutgoingLookup(element);
1116 forwardQueue_.
remove(element);
1121 auto vertexForwardQueueOutgoingLookup = vertex->getForwardQueueOutgoingLookup();
1122 for (
const auto &element : vertexForwardQueueOutgoingLookup)
1124 edgesToBeInserted_.emplace_back(element->data);
1125 vertex->removeFromForwardQueueOutgoingLookup(element);
1126 element->data.getChild()->removeFromForwardQueueIncomingLookup(element);
1127 forwardQueue_.
remove(element);
1132 for (
const auto &affectedVertex : affectedVertices)
1134 auto affectedVertexPtr = affectedVertex.lock();
1136 reverseSearchUpdateVertex(affectedVertexPtr);
1137 if (affectedVertex.lock()->hasReverseParent())
1139 insertOrUpdateInReverseQueue(affectedVertexPtr);
1140 affectedVertexPtr->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1148 vertex->setReverseParent(bestParent);
1151 bestParent->addToReverseChildren(vertex);
1154 vertex->setCostToComeFromGoal(
bestCost);
1157 if (!objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(),
1158 vertex->getExpandedCostToComeFromGoal()))
1160 insertOrUpdateInReverseQueue(vertex);
1165 auto reverseQueuePointer = vertex->getReverseQueuePointer();
1166 if (reverseQueuePointer)
1168 reverseQueue_.
remove(reverseQueuePointer);
1169 vertex->resetReverseQueuePointer();
1175 void AITstar::insertOrUpdateInReverseQueue(
const std::shared_ptr<aitstar::Vertex> &vertex)
1178 auto element = vertex->getReverseQueuePointer();
1183 element->data.first = computeSortKey(vertex);
1184 reverseQueue_.
update(element);
1189 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<aitstar::Vertex>> element(
1190 computeSortKey(vertex), vertex);
1193 auto reverseQueuePointer = reverseQueue_.
insert(element);
1194 vertex->setReverseQueuePointer(reverseQueuePointer);
1198 void AITstar::insertOrUpdateInForwardQueue(
const aitstar::Edge &edge)
1201 auto lookup = edge.getChild()->getForwardQueueIncomingLookup();
1202 auto it = std::find_if(lookup.begin(), lookup.end(), [&edge](
const auto element) {
1203 return element->data.getParent()->getId() == edge.getParent()->getId();
1206 if (it != lookup.end())
1210 assert(std::find_if(edge.getParent()->getForwardQueueOutgoingLookup().begin(),
1211 edge.getParent()->getForwardQueueOutgoingLookup().end(),
1212 [&edge](
const auto element) {
1213 return element->data.getChild()->getId() == edge.getChild()->getId();
1214 }) != edge.getParent()->getForwardQueueOutgoingLookup().end());
1215 (*it)->data.setSortKey(edge.getSortKey());
1216 forwardQueue_.
update(*it);
1220 auto element = forwardQueue_.
insert(edge);
1221 edge.getParent()->addToForwardQueueOutgoingLookup(element);
1222 edge.getChild()->addToForwardQueueIncomingLookup(element);
1226 std::shared_ptr<ompl::geometric::PathGeometric>
1227 AITstar::getPathToVertex(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1230 std::vector<std::shared_ptr<aitstar::Vertex>> reversePath;
1231 auto current = vertex;
1232 while (!graph_.
isStart(current))
1234 reversePath.emplace_back(current);
1235 current = current->getForwardParent();
1237 reversePath.emplace_back(current);
1240 auto path = std::make_shared<ompl::geometric::PathGeometric>(Planner::si_);
1241 for (
const auto &vertex : boost::adaptors::reverse(reversePath))
1243 path->append(vertex->getState());
1249 std::array<ompl::base::Cost, 3u> AITstar::computeSortKey(
const std::shared_ptr<aitstar::Vertex> &parent,
1250 const std::shared_ptr<aitstar::Vertex> &child)
const
1254 ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1256 objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1257 child->getCostToGoToGoal()),
1258 objective_->combineCosts(edgeCostHeuristic, child->getCostToGoToGoal()),
1259 parent->getCostToComeFromStart()};
1262 std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1265 return {objective_->combineCosts(objective_->betterCost(vertex->getCostToComeFromGoal(),
1266 vertex->getExpandedCostToComeFromGoal()),
1267 computeCostToGoToStartHeuristic(vertex)),
1268 objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1271 std::vector<aitstar::Edge> AITstar::getOutgoingEdges(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1274 std::vector<aitstar::Edge> outgoingEdges;
1277 for (
const auto &child : vertex->getForwardChildren())
1279 outgoingEdges.emplace_back(vertex, child, computeSortKey(vertex, child));
1283 for (
const auto &neighbor : graph_.
getNeighbors(vertex))
1286 if (vertex->getId() == neighbor->getId())
1292 if (vertex->hasReverseParent() && neighbor->getId() == vertex->getReverseParent()->getId())
1298 if (neighbor->isBlacklistedAsChild(vertex) || vertex->isBlacklistedAsChild(neighbor))
1304 outgoingEdges.emplace_back(vertex, neighbor, computeSortKey(vertex, neighbor));
1308 if (vertex->hasReverseParent())
1310 const auto &reverseParent = vertex->getReverseParent();
1311 outgoingEdges.emplace_back(vertex, reverseParent, computeSortKey(vertex, reverseParent));
1314 return outgoingEdges;
1317 bool AITstar::haveAllVerticesBeenProcessed(
const std::vector<aitstar::Edge> &edges)
const
1319 for (
const auto &edge : edges)
1321 if (!haveAllVerticesBeenProcessed(edge))
1330 bool AITstar::haveAllVerticesBeenProcessed(
const aitstar::Edge &edge)
const
1332 return edge.getParent()->hasBeenExpandedDuringCurrentReverseSearch() &&
1333 edge.getChild()->hasBeenExpandedDuringCurrentReverseSearch();
1336 void AITstar::updateExactSolution()
1343 if (objective_->isCostBetterThan(goal->getCostToComeFromStart(), solutionCost_) ||
1344 (!
pdef_->hasExactSolution() && objective_->isFinite(goal->getCostToComeFromStart())))
1347 solutionCost_ = goal->getCostToComeFromStart();
1351 solution.setPlannerName(
name_);
1354 solution.setOptimized(objective_, solutionCost_, objective_->isSatisfied(solutionCost_));
1357 pdef_->addSolutionPath(solution);
1360 informAboutNewSolution();
1365 void AITstar::updateApproximateSolution(
const std::shared_ptr<aitstar::Vertex> &vertex)
1367 assert(trackApproximateSolutions_);
1368 if (vertex->hasForwardParent() || graph_.
isStart(vertex))
1370 auto costToGoal = computeCostToGoToGoal(vertex);
1374 if (objective_->isCostBetterThan(costToGoal, approximateSolutionCostToGoal_) ||
1375 !
pdef_->hasApproximateSolution())
1378 approximateSolutionCost_ = vertex->getCostToComeFromStart();
1379 approximateSolutionCostToGoal_ = costToGoal;
1381 solution.setPlannerName(
name_);
1384 solution.setApproximate(costToGoal.value());
1387 solution.setOptimized(objective_, approximateSolutionCost_,
false);
1390 pdef_->addSolutionPath(solution);
1395 ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1402 bestCost, objective_->motionCostHeuristic(vertex->getState(), start->getState()));
1407 ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1414 bestCost, objective_->motionCostHeuristic(vertex->getState(), goal->getState()));
1419 ompl::base::Cost AITstar::computeCostToGoToGoal(
const std::shared_ptr<aitstar::Vertex> &vertex)
const
1426 objective_->betterCost(
bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1437 bestCost = objective_->betterCost(
bestCost, start->getCostToComeFromGoal());
1442 std::size_t AITstar::countNumVerticesInForwardTree()
const
1444 std::size_t numVerticesInForwardTree = 0u;
1446 for (
const auto &vertex : vertices)
1448 if (graph_.
isStart(vertex) || vertex->hasForwardParent())
1450 ++numVerticesInForwardTree;
1453 return numVerticesInForwardTree;
1456 std::size_t AITstar::countNumVerticesInReverseTree()
const
1458 std::size_t numVerticesInReverseTree = 0u;
1460 for (
const auto &vertex : vertices)
1462 if (graph_.
isGoal(vertex) || vertex->hasReverseParent())
1464 ++numVerticesInReverseTree;
1467 return numVerticesInReverseTree;
1470 void AITstar::invalidateCostToComeFromGoalOfReverseBranch(
const std::shared_ptr<aitstar::Vertex> &vertex)
1472 vertex->unregisterExpansionDuringReverseSearch();
1474 for (
const auto &child : vertex->getReverseChildren())
1476 invalidateCostToComeFromGoalOfReverseBranch(child);
1477 child->setCostToComeFromGoal(objective_->infiniteCost());
1478 child->setExpandedCostToComeFromGoal(objective_->infiniteCost());
1479 auto reverseQueuePointer = child->getReverseQueuePointer();
1480 if (reverseQueuePointer)
1482 reverseQueue_.
remove(reverseQueuePointer);
1483 child->resetReverseQueuePointer();
void pop()
Remove the top element.
bool empty() const
Check if the heap is empty.
void remove(Element *element)
Remove a specific element.
Element * top() const
Return the top element. nullptr for an empty heap.
Element * insert(const _T &data)
Add a new 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.
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.
bool setup_
Flag indicating whether setup() has been called.
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
std::vector< aitstar::Edge > getEdgesInQueue() const
Get the edge queue.
void enablePruning(bool prune)
Set whether pruning is enabled or not.
aitstar::Edge getNextEdgeInQueue() const
Get the next edge in the queue.
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.
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
void setBatchSize(std::size_t batchSize)
Set the batch size.
ompl::base::Cost bestCost() const
Get the cost of the incumbent solution.
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
void setRepairReverseSearch(bool repairReverseSearch)
Enable LPA* repair of reverse search.
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
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.
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::size_t getBatchSize() const
Get the batch size.
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
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.
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 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 clear()
Resets the graph to its construction state, without resetting options.
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
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.
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
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_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.