Loading...
Searching...
No Matches
AITstar.cpp
72 declareParam<bool>("use_k_nearest", this, &AITstar::setUseKNearest, &AITstar::getUseKNearest, "0,1");
73 declareParam<double>("rewire_factor", this, &AITstar::setRewireFactor, &AITstar::getRewireFactor,
75 declareParam<std::size_t>("samples_per_batch", this, &AITstar::setBatchSize, &AITstar::getBatchSize,
77 declareParam<bool>("use_graph_pruning", this, &AITstar::enablePruning, &AITstar::isPruningEnabled, "0,1");
84 addPlannerProgressProperty("iterations INTEGER", [this]() { return std::to_string(numIterations_); });
169 AITstar::ensureStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition)
215 ompl::base::PlannerStatus AITstar::solve(const ompl::base::PlannerTerminationCondition &terminationCondition)
236 OMPL_INFORM("%s: Solving the given planning problem. The current best solution cost is %.4f", name_.c_str(),
245 // Someone might call ProblemDefinition::clearSolutionPaths() between invocations of Planner::sovle(), in
388 // Insert all edges into the queue if they connect vertices that have been processed, otherwise store
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 "
513 name_.c_str(), numIterations_, graph_.getNumberOfSampledStates(), graph_.getNumberOfValidSamples(),
534 aitstar::KeyVertexPair element({computeCostToGoToStartHeuristic(goal), objective_->identityCost()},
564 // The reverse search must be continued if the best edge has an inconsistent child state or if the best
579 // If the best edge in the forward queue has a potential total solution cost of infinity, the forward
580 // search does not need to be continued. This can happen if the reverse search did not reach any target
664 } // If the reverse search is suspended, check whether the forward search needs to be continued.
668 } // If neither the forward search nor the reverse search needs to be continued, add more samples.
725 else if (objective_->isCostBetterThan(objective_->combineCosts(parent->getCostToComeFromStart(),
796 if (objective_->isCostBetterThan(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal()))
819 lhs.getSortKey().cbegin(), lhs.getSortKey().cend(), rhs.getSortKey().cbegin(), rhs.getSortKey().cend(),
823 bool AITstar::isVertexBetter(const aitstar::KeyVertexPair &lhs, const aitstar::KeyVertexPair &rhs) const
825 // If the costs of two vertices are equal then we prioritize inconsistent vertices that are targets of
846 assert(objective_->isCostEquivalentTo(vertex->getCostToComeFromGoal(), objective_->identityCost()));
852 auto bestCost = vertex->hasReverseParent() ? vertex->getCostToComeFromGoal() : objective_->infiniteCost();
861 auto parentCost = objective_->combineCosts(neighbor->getExpandedCostToComeFromGoal(), edgeCost);
874 auto parentCost = objective_->combineCosts(forwardChild->getExpandedCostToComeFromGoal(), edgeCost);
888 auto parentCost = objective_->combineCosts(forwardParent->getExpandedCostToComeFromGoal(), edgeCost);
932 // This vertex now has a changed cost-to-come in the reverse search. All edges in the forward queue that
933 // have this vertex as a target must be updated. This cannot be delayed, as whether the reverse search
945 // Start with the reverse search children, because if this vertex becomes the parent of a neighbor, that
989 std::pair<std::array<ompl::base::Cost, 2u>, std::shared_ptr<Vertex>> element(computeSortKey(vertex),
1008 // We used the incoming lookup of the child. Assert that it is already in the outgoing lookup of the
1030 if (!edge.getChild()->isConsistent() || !objective_->isFinite(edge.getChild()->getCostToComeFromGoal()))
1068 std::array<ompl::base::Cost, 3u> AITstar::computeSortKey(const std::shared_ptr<Vertex> &parent,
1073 ompl::base::Cost edgeCostHeuristic = objective_->motionCostHeuristic(parent->getState(), child->getState());
1075 objective_->combineCosts(objective_->combineCosts(parent->getCostToComeFromStart(), edgeCostHeuristic),
1081 std::array<ompl::base::Cost, 2u> AITstar::computeSortKey(const std::shared_ptr<Vertex> &vertex) const
1087 objective_->betterCost(vertex->getCostToComeFromGoal(), vertex->getExpandedCostToComeFromGoal())};
1141 // We need to check whether the cost is better, or whether someone has removed the exact solution
1169 start->callOnForwardBranch([this](const auto &vertex) -> void { updateApproximateSolution(vertex); });
1180 // We need to check whether this is better than the current approximate solution or whether someone
1221 ompl::base::PlannerStatus::StatusType AITstar::updateSolution(const std::shared_ptr<Vertex> &vertex)
1239 ompl::base::Cost AITstar::computeCostToGoToStartHeuristic(const std::shared_ptr<Vertex> &vertex) const
1251 ompl::base::Cost AITstar::computeCostToGoToGoalHeuristic(const std::shared_ptr<Vertex> &vertex) const
1270 objective_->betterCost(bestCost, objective_->motionCost(vertex->getState(), goal->getState()));
1314 void AITstar::invalidateCostToComeFromGoalOfReverseBranch(const std::shared_ptr<Vertex> &vertex)
1316 // If this vertex is consistent before invalidation, then all incoming edges now have targets that are
1323 // Reset the cost to come from the goal and the reverse parent unless the vertex is itself a goal.
void getContent(std::vector< _T > &content) const
Get the data stored in this heap.
Definition BinaryHeap.h:207
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition PlannerData.h:59
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition PlannerData.h:175
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...
Definition PlannerData.cpp:413
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...
Definition PlannerData.cpp:422
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...
Definition PlannerData.cpp:432
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
Definition PlannerData.cpp:391
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition Planner.cpp:339
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition Planner.cpp:346
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition PlannerTerminationCondition.h:64
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition Planner.cpp:106
A shared pointer wrapper for ompl::base::SpaceInformation.
std::shared_ptr< aitstar::Vertex > getNextVertexInQueue() const
Get the next vertex in the queue.
Definition AITstar.cpp:624
void setMaxNumberOfGoals(unsigned int numberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
Definition AITstar.cpp:361
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG graph.
Definition AITstar.cpp:313
ompl::base::PlannerStatus::StatusType ensureStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition)
Checks whether the problem is successfully setup.
Definition AITstar.cpp:169
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
Definition AITstar.cpp:366
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInQueue() const
Get the vertex queue.
Definition AITstar.cpp:599
AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)
Constructs a AIT*.
Definition AITstar.cpp:55
bool getUseKNearest() const
Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition AITstar.cpp:356
void getPlannerData(base::PlannerData &data) const override
Get the planner data.
Definition AITstar.cpp:259
void setup() override
Additional setup that can only be done once a problem definition is set.
Definition AITstar.cpp:94
ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override
Solves a motion planning problem.
Definition AITstar.cpp:215
ompl::base::PlannerStatus::StatusType ensureSetup()
Checks whether the planner is successfully setup.
Definition AITstar.cpp:145
std::vector< std::shared_ptr< aitstar::Vertex > > getVerticesInReverseSearchTree() const
Get the vertices in the reverse search tree.
Definition AITstar.cpp:634
void trackApproximateSolutions(bool track)
Set whether to track approximate solutions.
Definition AITstar.cpp:323
void setUseKNearest(bool useKNearest)
Set whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.
Definition AITstar.cpp:351
bool areApproximateSolutionsTracked() const
Get whether approximate solutions are tracked.
Definition AITstar.cpp:336
Definition Edge.h:56
const std::array< ompl::base::Cost, 3u > & getSortKey() const
Returns the sort key associated with this edge.
Definition Edge.cpp:70
void setSortKey(const std::array< ompl::base::Cost, 3u > &key)
Sets the sort key associated with this edge.
Definition Edge.cpp:75
bool isStart(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a start vertex.
Definition ImplicitGraph.cpp:415
bool hasAStartState() const
Returns whether the graph has a goal state.
Definition ImplicitGraph.cpp:148
const std::vector< std::shared_ptr< Vertex > > & getStartVertices() const
Get the start vertices.
Definition ImplicitGraph.cpp:439
std::vector< std::shared_ptr< Vertex > > getVertices() const
Get all vertices.
Definition ImplicitGraph.cpp:449
void setMaxNumberOfGoals(unsigned int maxNumberOfGoals)
Set the maximum number of goals AIT* will sample from sampleable goal regions.
Definition ImplicitGraph.cpp:95
bool isGoal(const std::shared_ptr< Vertex > &vertex) const
Checks whether the vertex is a goal vertex.
Definition ImplicitGraph.cpp:427
std::size_t getNumberOfValidSamples() const
Returns the total number of valid samples found.
Definition ImplicitGraph.cpp:520
void prune()
Prune all samples that can not contribute to a solution better than the current one.
Definition ImplicitGraph.cpp:456
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.
Definition ImplicitGraph.cpp:159
void clear()
Resets the graph to its construction state, without resetting options.
Definition ImplicitGraph.cpp:71
const std::vector< std::shared_ptr< Vertex > > & getGoalVertices() const
Get the goal vertices.
Definition ImplicitGraph.cpp:444
bool addSamples(std::size_t numNewSamples, const ompl::base::PlannerTerminationCondition &terminationCondition)
Adds a batch of samples and returns the samples it has added.
Definition ImplicitGraph.cpp:315
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.
Definition ImplicitGraph.cpp:55
std::vector< std::shared_ptr< Vertex > > getNeighbors(const std::shared_ptr< Vertex > &vertex) const
Get neighbors of a vertex.
Definition ImplicitGraph.cpp:391
bool hasAGoalState() const
Returns whether the graph has a goal state.
Definition ImplicitGraph.cpp:153
bool getUseKNearest() const
Whether the graph uses a k-nearest connection model. If false, it uses an r-disc model.
Definition ImplicitGraph.cpp:110
std::size_t getNumberOfSampledStates() const
Returns the total number of sampled states.
Definition ImplicitGraph.cpp:515
void setRewireFactor(double rewireFactor)
Set the rewire factor of the RGG.
Definition ImplicitGraph.cpp:85
unsigned int getMaxNumberOfGoals() const
Get the maximum number of goals AIT* will sample from sampleable goal regions.
Definition ImplicitGraph.cpp:100
void setUseKNearest(bool useKNearest)
Whether to use a k-nearest connection model. If false, it uses an r-disc model.
Definition ImplicitGraph.cpp:105
std::vector< EdgeQueue::Element * > getForwardQueueIncomingLookup() const
Returns the forward queue incoming lookup of this vertex.
Definition Vertex.cpp:511
void addToForwardQueueOutgoingLookup(typename EdgeQueue::Element *pointer)
Adds an element to the forward queue outgoing lookup.
Definition Vertex.cpp:502
std::vector< EdgeQueue::Element * > getForwardQueueOutgoingLookup() const
Returns the forward queue outgoing lookup of this vertex.
Definition Vertex.cpp:518
bool isConsistent() const
Returns whether the vertex is consistent, i.e., whether its cost-to-come is equal to the cost-to-come...
Definition Vertex.cpp:459
ompl::base::Cost getCostToComeFromGoal() const
Returns the cost to come to this vertex from the goal.
Definition Vertex.cpp:134
void addToForwardQueueIncomingLookup(typename EdgeQueue::Element *pointer)
Adds an element to the forward queue incoming lookup.
Definition Vertex.cpp:495
void removeFromForwardQueueIncomingLookup(typename EdgeQueue::Element *element)
Remove an element from the incoming queue lookup.
Definition Vertex.cpp:523
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
Definition PlannerTerminationCondition.cpp:190
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition GoalTypes.h:56
Main namespace. Contains everything in this library.
Definition MultiLevelPlanarManipulatorDemo.cpp:66
Representation of a solution to a planning problem.
Definition ProblemDefinition.h:70
A class to store the exit status of Planner::solve()
Definition PlannerStatus.h:49
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
Definition PlannerStatus.h:60