Class AITstar

Inheritance Relationships

Base Type

Class Documentation

class AITstar : public ompl::base::Planner

Adaptively Informed Trees (AIT*)

AIT* (Adaptively Informed Trees) is an almost-surely asymptotically optimal path planner. It aims to find an initial solution quickly and asymptotically converge to the globally optimal solution.

AIT* views the planning problem as the two subproblems of approximation and search, and approximates the free state space with an increasingly dense, edge-implicit random geometric graph (RGG), similar to BIT* and ABIT*.

AIT* uses an asymmetric bidirectional search that simultaneously estimates and exploits an accurate, adaptive heuristic that is specific to each problem instance. The reverse search (goal to start) does not perform collision detection on the edges and estimates cost-to-go values for all states in the RGG that are processed with the forward search (start to goal) which does perform collision detection. Because AIT* uses LPA* as the reverse search algorithm, the heuristic is admissible in the context of the current RGG and can efficiently be updated when the forward search detects a collision on an edge that is in the reverse search tree (as this indicates a flawed heuristic).

This implementation of AIT* can solve problems with multiple start and goal states and supports adding start and goal states while the planner is running. One can also turn off repairing the reverse search tree upon collision detection, which might be beneficial for problems in which collision detection is computationally inexpensive.

M. P. Strub, J. D. Gammell. “Adaptively Informed Trees (AIT*): Fast Asymptotically Optimal Path Planning through Adaptive Heuristics” in Proceedings of the IEEE international conference on robotics and automation (ICRA), Paris, France, 31 May – 4 Jun. 2020.

Associated publications:

arXiv: arXiv:2002.06589 DOI: ICRA40945.2020.9197338 Video 1: ICRA submission video. Video 2: ICRA presentation video

M. P. Strub, J. D. Gammell. “AIT* and EIT*: Asymmetric bidirectional sampling-based path planning” The International Journal of Robotics Research (IJRR), in revision, 2021.

arXiv: arXiv:2111.01877 Video 1: IJRR trailer Video 2: IJRR extension 1

Public Functions

explicit AITstar(const ompl::base::SpaceInformationPtr &spaceInformation)

Constructs a AIT*.

~AITstar() = default

Destructs a AIT*.

virtual void setup() override

Additional setup that can only be done once a problem definition is set.

ompl::base::PlannerStatus::StatusType ensureSetup()

Checks whether the planner is successfully setup.

ompl::base::PlannerStatus::StatusType ensureStartAndGoalStates(const ompl::base::PlannerTerminationCondition &terminationCondition)

Checks whether the problem is successfully setup.

virtual void clear() override

Clears the algorithm’s internal state.

virtual ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &terminationCondition) override

Solves a motion planning problem.

ompl::base::Cost bestCost() const

Get the cost of the incumbent solution.

virtual void getPlannerData(base::PlannerData &data) const override

Get the planner data.

void setBatchSize(std::size_t batchSize)

Set the batch size.

std::size_t getBatchSize() const

Get the batch size.

void setRewireFactor(double rewireFactor)

Set the rewire factor of the RGG graph.

double getRewireFactor() const

Get the rewire factor of the RGG graph.

void trackApproximateSolutions(bool track)

Set whether to track approximate solutions.

bool areApproximateSolutionsTracked() const

Get whether approximate solutions are tracked.

void enablePruning(bool prune)

Set whether pruning is enabled or not.

bool isPruningEnabled() const

Get whether pruning is enabled or not.

void setUseKNearest(bool useKNearest)

Set whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.

bool getUseKNearest() const

Get whether to use a k-nearest RGG connection model. If false, AIT* uses an r-disc model.

void setMaxNumberOfGoals(unsigned int numberOfGoals)

Set the maximum number of goals AIT* will sample from sampleable goal regions.

unsigned int getMaxNumberOfGoals() const

Get the maximum number of goals AIT* will sample from sampleable goal regions.

std::vector<aitstar::Edge> getEdgesInQueue() const

Get the edge queue.

std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInQueue() const

Get the vertex queue.

aitstar::Edge getNextEdgeInQueue() const

Get the next edge in the queue.

std::shared_ptr<aitstar::Vertex> getNextVertexInQueue() const

Get the next vertex in the queue.

std::vector<std::shared_ptr<aitstar::Vertex>> getVerticesInReverseSearchTree() const

Get the vertices in the reverse search tree.