Class BLITstar

Inheritance Relationships

Base Type

Class Documentation

class BLITstar : public ompl::base::Planner

BLIT* (Bidirectional Lazy Informed Trees) is a novel almost-surely asymptotically optimal motion planner.

 BLIT* is the first algorithm that incorporate anytime incremental lazy bidrectional heuristic search into batch-wise 

 sampling-based motion planning. BLIT* introduces the first anytime incremental bidirectional heuristic search and develops

 a new lazy edge evaluation strategy.

Public Functions

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

Constructs a BLIT*.

~BLITstar()

Destructs a BLIT*.

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 costCombine(ompl::base::Cost c1, ompl::base::Cost c2)

Operate + .

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 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, BLIT* uses an r-disc model.

bool getUseKNearest() const

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

void setMaxNumberOfGoals(unsigned int numberOfGoals)

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

unsigned int getMaxNumberOfGoals() const

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

void runTime()
bool SCD(const blitstar::keyEdgePair &edge)

Above references inherit from BLIT*.

Perform sparse/compelete collision detection.

bool CCD(const blitstar::keyEdgePair &edge)
void clearReverseVertexQueue()

Empty the queues

void clearForwardVertexQueue()
void resetReverseValue(const std::shared_ptr<blitstar::Vertex> &vertex)

Reset a vertex’s value.

void resetForwardValue(const std::shared_ptr<blitstar::Vertex> &vertex)
bool terminateSearch()

Ensuring meet-in-the-middle and optimality to terminate the current search.

void insertGoalVerticesInReverseVertexQueue()

Insert start and goal vertices into the queues.

void insertStartVerticesInForWardVertexQueue()
bool SelectExpandState(bool &forward)

Select the vertex with minimal priority on both trees.

void resetForwardParentInformation(const std::shared_ptr<blitstar::Vertex> &vertex)

Reset parent vertex’s information.

void resetReverseParentInformation(const std::shared_ptr<blitstar::Vertex> &vertex)
void resetForwardParentAndRemeberTheVertex(const std::shared_ptr<blitstar::Vertex> &child, const std::shared_ptr<blitstar::Vertex> &parent)
void resetReverseParentAndRemeberTheVertex(const std::shared_ptr<blitstar::Vertex> &child, const std::shared_ptr<blitstar::Vertex> &parent)
void lookingForBestNeighbor(ompl::base::Cost curMin_, size_t neighbor)

Look for a neighbor with the minimal priority.

void bestNeighbor(ompl::base::Cost costToCome, ompl::base::Cost costToGoal, size_t neighbor)
void ForwardLazySearch(const std::shared_ptr<blitstar::Vertex> &vertex)

Forward and Reverse Search.

void ReverseLazySearch(const std::shared_ptr<blitstar::Vertex> &vertex)
bool PathValidity(std::shared_ptr<blitstar::Vertex> &vertex)

Checking the validity of a path from each direction.

void ForwardPathValidityChecking(std::shared_ptr<blitstar::Vertex> &vertex, bool &validity)
void ReversePathValidityChecking(std::shared_ptr<blitstar::Vertex> &vertex, bool &validity)
bool isValidAtResolution(const blitstar::keyEdgePair &edge, std::size_t numChecks, bool sparseCheck)

Checking the collision detection.

void EvaluateValidityStartAndToGoal(const std::shared_ptr<blitstar::Vertex> &start, const std::shared_ptr<blitstar::Vertex> &goal)

Checking the collision detection between start and goal vertices.

void insertOrUpdateInForwardVertexQueue(const std::shared_ptr<blitstar::Vertex> &vertex, ompl::base::Cost CostToCome, ompl::base::Cost CostToGoal, bool couldMeet)

Inserts or updates a vertex in the reverse queue.

void insertOrUpdateInReverseVertexQueue(const std::shared_ptr<blitstar::Vertex> &vertex, ompl::base::Cost CostToCome, ompl::base::Cost CostToGoal, bool couldMeet)
void updateReverseCost(const std::shared_ptr<blitstar::Vertex> &vertex, ompl::base::Cost costToCome, ompl::base::Cost &costToGo)

Refine heuristics on-the-fly.

void updateForwardCost(const std::shared_ptr<blitstar::Vertex> &vertex, ompl::base::Cost costToCome, ompl::base::Cost &costToGo)
void updateCostToGo(ompl::base::Cost &costToCome, ompl::base::Cost &costToGo, ompl::base::Cost costFromOriginal, bool meetOnTree)
void updateBestSolutionFoundSoFar(const std::shared_ptr<blitstar::Vertex> &vertex, ompl::base::Cost meetCost, ompl::base::Cost costToCome, ompl::base::Cost &costToGo, ompl::base::Cost costFromOri)

Improve the current solution.