ompl::geometric::EITstar Class Reference

Effort Informed Trees (EIT*) More...

#include <ompl/geometric/planners/informedtrees/EITstar.h>

Inheritance diagram for ompl::geometric::EITstar:

Public Member Functions

 EITstar (const std::shared_ptr< ompl::base::SpaceInformation > &spaceInfo)
 Constructs an instance of EIT* using the provided space information.
 
 ~EITstar ()
 Destructs this instance of EIT*.
 
void setup () override
 Setup the parts of the planner that rely on the problem definition being set.
 
ompl::base::PlannerStatus solve (const ompl::base::PlannerTerminationCondition &terminationCondition) override
 Solves the provided motion planning problem, respecting the given termination condition.
 
void clear () override
 Clears all internal planner structures but retains settings. Subsequent calls to solve() will start from scratch.
 
void clearQuery () override
 Clears all query-specific information, such as start and goal states and search trees. EIT* retains the samples and collision checking cache of previous queries.
 
ompl::base::Cost bestCost () const
 Returns the cost of the current best solution.
 
void setBatchSize (unsigned int numSamples)
 Sets the number of samples per batch.
 
unsigned int getBatchSize () const
 Returns the number of samples per batch.
 
void setInitialNumberOfSparseCollisionChecks (std::size_t numChecks)
 Sets the initial number of collision checks on the reverse search.
 
void setRadiusFactor (double factor)
 Sets the radius factor.
 
double getRadiusFactor () const
 Returns the radius factor.
 
void setSuboptimalityFactor (double factor)
 Sets the (initial) suboptimality factor.
 
void enablePruning (bool prune)
 Set whether pruning is enabled or not.
 
bool isPruningEnabled () const
 Returns whether pruning is enabled or not.
 
void trackApproximateSolutions (bool track)
 Sets whether to track approximate solutions or not.
 
bool areApproximateSolutionsTracked () const
 Returns whether approximate solutions are tracked or not.
 
void setUseKNearest (bool useKNearest)
 Set whether to use a k-nearest RGG connection model. If false, EIT* uses an r-disc model.
 
bool getUseKNearest () const
 Returns whether to use a k-nearest RGG connection model. If false, EIT* uses an r-disc model.
 
void setMaxNumberOfGoals (unsigned int numberOfGoals)
 Set the maximum number of goals EIT* will sample from sampleable goal regions.
 
unsigned int getMaxNumberOfGoals () const
 Returns the maximum number of goals EIT* will sample from sampleable goal regions.
 
bool isForwardQueueEmpty () const
 Returns true if the forward queue is empty.
 
std::vector< eitstar::EdgegetForwardQueue () const
 Returns a copy of the forward queue.
 
unsigned int getForwardEffort () const
 Returns the effort of the edge at the top of the forward queue.
 
bool isReverseQueueEmpty () const
 Returns true if the reverse queue is empty.
 
std::vector< eitstar::EdgegetReverseQueue () const
 Returns a copy of the reverse queue.
 
std::vector< eitstar::EdgegetReverseTree () const
 Returns copies of the edges in the reverse tree.
 
eitstar::Edge getNextForwardEdge () const
 Returns the next edge in the forward queue.
 
eitstar::Edge getNextReverseEdge () const
 Returns the next edge in the reverse queue.
 
bool isStart (const std::shared_ptr< eitstar::State > &state) const
 Checks whether the state is a start state.
 
bool isGoal (const std::shared_ptr< eitstar::State > &state) const
 Checks whether the state is a goal state.
 
void getPlannerData (base::PlannerData &data) const override
 Returns the planner data.
 
- Public Member Functions inherited from ompl::base::Planner
 Planner (const Planner &)=delete
 
Planneroperator= (const Planner &)=delete
 
 Planner (SpaceInformationPtr si, std::string name)
 Constructor.
 
virtual ~Planner ()=default
 Destructor.
 
template<class T >
T * as ()
 Cast this instance to a desired type. More...
 
template<class T >
const T * as () const
 Cast this instance to a desired type. More...
 
const SpaceInformationPtrgetSpaceInformation () const
 Get the space information this planner is using.
 
const ProblemDefinitionPtrgetProblemDefinition () const
 Get the problem definition the planner is trying to solve.
 
ProblemDefinitionPtrgetProblemDefinition ()
 Get the problem definition the planner is trying to solve.
 
const PlannerInputStatesgetPlannerInputStates () const
 Get the planner input states.
 
virtual void setProblemDefinition (const ProblemDefinitionPtr &pdef)
 Set the problem definition for the planner. The problem needs to be set before calling solve(). Note: If this problem definition replaces a previous one, it may also be necessary to call clear() or clearQuery().
 
PlannerStatus solve (const PlannerTerminationConditionFn &ptc, double checkInterval)
 Same as above except the termination condition is only evaluated at a specified interval.
 
PlannerStatus solve (double solveTime)
 Same as above except the termination condition is solely a time limit: the number of seconds the algorithm is allowed to spend planning.
 
const std::string & getName () const
 Get the name of the planner.
 
void setName (const std::string &name)
 Set the name of the planner.
 
const PlannerSpecsgetSpecs () const
 Return the specifications (capabilities of this planner)
 
virtual void checkValidity ()
 Check to see if the planner is in a working state (setup has been called, a goal was set, the input states seem to be in order). In case of error, this function throws an exception.
 
bool isSetup () const
 Check if setup() was called for this planner.
 
ParamSetparams ()
 Get the parameters for this planner.
 
const ParamSetparams () const
 Get the parameters for this planner.
 
const PlannerProgressPropertiesgetPlannerProgressProperties () const
 Retrieve a planner's planner progress property map.
 
virtual void printProperties (std::ostream &out) const
 Print properties of the motion planner.
 
virtual void printSettings (std::ostream &out) const
 Print information about the motion planner's settings.
 

Protected Member Functions

void enableMultiquery (bool multiquery)
 Set wheter multiquery is enabled or not.
 
bool isMultiqueryEnabled () const
 Get wheter multiquery is enabled or not.
 
void setStartGoalPruningThreshold (unsigned int threshold)
 Set start/goal pruning threshold.
 
unsigned int getStartGoalPruningThreshold () const
 Get threshold at which we prune starts/goals.
 
- Protected Member Functions inherited from ompl::base::Planner
template<typename T , typename PlannerType , typename SetterType , typename GetterType >
void declareParam (const std::string &name, const PlannerType &planner, const SetterType &setter, const GetterType &getter, const std::string &rangeSuggestion="")
 This function declares a parameter for this planner instance, and specifies the setter and getter functions.
 
template<typename T , typename PlannerType , typename SetterType >
void declareParam (const std::string &name, const PlannerType &planner, const SetterType &setter, const std::string &rangeSuggestion="")
 This function declares a parameter for this planner instance, and specifies the setter function.
 
void addPlannerProgressProperty (const std::string &progressPropertyName, const PlannerProgressProperty &prop)
 Add a planner progress property called progressPropertyName with a property querying function prop to this planner's progress property map.
 

Additional Inherited Members

- Public Types inherited from ompl::base::Planner
using PlannerProgressProperty = std::function< std::string()>
 Definition of a function which returns a property about the planner's progress that can be queried by a benchmarking routine.
 
using PlannerProgressProperties = std::map< std::string, PlannerProgressProperty >
 A dictionary which maps the name of a progress property to the function to be used for querying that property.
 
- Protected Attributes inherited from ompl::base::Planner
SpaceInformationPtr si_
 The space information for which planning is done.
 
ProblemDefinitionPtr pdef_
 The user set problem definition.
 
PlannerInputStates pis_
 Utility class to extract valid input states

 
std::string name_
 The name of this planner.
 
PlannerSpecs specs_
 The specifications of the planner (its capabilities)
 
ParamSet params_
 A map from parameter names to parameter instances for this planner. This field is populated by the declareParam() function.
 
PlannerProgressProperties plannerProgressProperties_
 A mapping between this planner's progress property names and the functions used for querying those progress properties.
 
bool setup_
 Flag indicating whether setup() has been called.
 

Detailed Description

Effort Informed Trees (EIT*)

EIT* (Effort 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.

EIT* 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 AIT*.

EIT* uses an asymmetric bidirectional search that simultaneously calculates and exploits accurate, problem-specific cost and effort heuristics. The reverse search (goal to start) only performs sparse collision detection on the edges and does not evaluate true edge costs. It calculates cost-to-go and effort-to-go heuristics for all states in the RGG that are processed with the forward search (start to goal). This is achieved by integrating a priori cost and effort heuristics over the best reverse path to each state. The forward search (start to goal) performs collision detection and is informed by the heuristics calculated by the reverse search. The forward search in turn informes the reverse search when it detects collisions on an edge that was used to calculate the heuristics which causes the reverse search to update them. In this way, both searches in EIT* continuously inform each other with complementary information. This results in fast initial solution times and almost-sure asymptotic convergence towards the optimal solution.

This implementation of EIT* can solve problems with multiple start and goal states and supports adding start and goal states while the planner is running.

Associated publications:

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 2

Definition at line 155 of file EITstar.h.


The documentation for this class was generated from the following files:
  • ompl/geometric/planners/informedtrees/EITstar.h
  • ompl/geometric/planners/informedtrees/src/EITstar.cpp