Class AnytimePathShortening

Inheritance Relationships

Base Type

Class Documentation

class AnytimePathShortening : public ompl::base::Planner

Short description

Anytime path shortening is a generic wrapper around one or more geometric motion planners that repeatedly applies shortcutting (ompl::geometric::PathSimplifier) and hybridization (ompl::geometric::PathHybridization) to a set of solution paths with the goal of rapidly converging to a solution with minimal length. Any number and combination of planners can be specified, each is run in a separate thread. A path length objective may be set, otherwise the tool will run until the termination condition is met. The purpose of this tool is to add anytime properties to motion planners that are not typically viewed as optimal/optimizing algorithms. This implementation deviates from the published version. Here, n threads repeatedly produce solution paths (that are optionally shortcutted), which are then subsequently added to a shared pool of solutions paths. The threads run independently and don’t need to synchronize. A seperate thread repeatedly applies path hybridization to the top paths and, optionally, applies path simplification to the best path found so far.

External documentation

R. Luna, I.A. Şucan, M. Moll, and L.E. Kavraki, Anytime Solution Optimization for Sampling-Based Motion Planning, in Proc. 2013 IEEE Intl. Conf. on Robotics and Automation, pp. 5053-5059, May. 2013. DOI: ICRA.2013.6631301[PDF]

Public Functions

AnytimePathShortening(const base::SpaceInformationPtr &si)

Constructor requires the space information to plan in.

~AnytimePathShortening() override

Destructor.

void addPlanner(base::PlannerPtr &planner)

Adds the given planner to the set of planners used to compute candidate paths.

virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override

Method that solves the motion planning problem. This method terminates under just two conditions, the given argument condition, or when the maximum path length in the optimization objective is met.

Remark

This method spawns a separate thread for each planner employed, and applies a series of optimization methods to the set of paths generated by each planning thread.

virtual void clear() override

Clear all internal planning datastructures. Planner settings are not affected. Subsequent calls to solve() will ignore all previous work.

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

Get information about the most recent run of the motion planner.

Remark

This call is ambiguous in this tool. By default, the planner data for the first planner in the planner list is returned.

virtual void getPlannerData(ompl::base::PlannerData &data, unsigned int idx) const

Get information about the most recent run of the idxth motion planner.

virtual void setup() override

Perform any necessary configuration steps. This method also invokes ompl::base::SpaceInformation::setup() if needed. This must be called before solving.

virtual void checkValidity() override

Check to see if the planners are 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.

unsigned int getNumPlanners() const

Retrieve the number of planners added.

base::PlannerPtr getPlanner(unsigned int idx) const

Retrieve a pointer to the ith planner instance.

bool isShortcutting() const

Return whether the anytime planner will perform shortcutting on paths.

void setShortcut(bool shortcut)

Enable/disable shortcutting on paths.

bool isHybridizing() const

Return whether the anytime planner will extract a hybrid path from the set of solution paths.

void setHybridize(bool hybridize)

Enable/disable path hybridization on the set of solution paths.

unsigned int maxHybridizationPaths() const

Return the maximum number of paths that will be hybridized.

void setMaxHybridizationPath(unsigned int maxPathCount)

Set the maximum number of paths that will be hybridized.

void setPlanners(const std::string &plannerList)

Set the list of planners to use.

This will make the list of planners equal to PRM, EST, and RRT. Optionally, planner parameters can be passed to change the default: “PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]” Use spaces to separate multiple parameter settings, as shown in the example.

Parameters:

plannerList – A string containing a comma-separated list of planner names, e.g., “PRM,EST,RRT”

std::string getPlanners() const

Get a string representation of the planners and their parameters in the format of setPlanners.

void setDefaultNumPlanners(unsigned int numPlanners)

Set default number of planners to use if none are specified.

unsigned int getDefaultNumPlanners() const

Get default number of planners used if none are specified.

std::string getBestCost() const

Return best cost found so far by algorithm.

virtual void printSettings(std::ostream &out) const override

Print settings of this planner as well as those of the planner instances it contains.

Public Static Functions

template<typename PlannerType>
static inline std::shared_ptr<AnytimePathShortening> createPlanner(const base::SpaceInformationPtr &si, unsigned int numPlanners = std::max(1u, std::thread::hardware_concurrency()))

Factory for creating a shared pointer to an AnytimePathShortening instance with numPlanners instances of planners of type PlannerType.

template<typename ...PlannerTypes>
static inline std::shared_ptr<AnytimePathShortening> createPlanner(const base::SpaceInformationPtr &si)

Factory for creating a shared pointer to an AnytimePathShortening instance with planners of type PlannerType1, PlannerType2, …

Example: createPlanner<ompl::geometric::PRM, ompl::geometric::RRT, ompl::geometric::EST> would return a shared pointer to an AnytimePathShortening instance containing a PRM, RRT, and EST instance. Typenames can be repeated to get multiple planner instances of that type.

Protected Functions

void addPath(const geometric::PathGeometricPtr &path, base::Planner *planner)

add a path to set of solutions

Parameters:

  • path – solution path

  • planner – planner that produced the solution. If planner==this, the path is the result of hybridization/simplification and is only added if it improves the best known solution.

virtual void threadSolve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)

The function that the planning threads execute when solving a motion planning problem.

Protected Attributes

std::vector<base::PlannerPtr> planners_

The list of planners used for solving the problem.

unsigned int invalidStartStateCount_ = {0}

The number of times (1 per planner) INVALID_START_STATE is returned by a planner.

unsigned int invalidGoalCount_ = {0}

The number of times (1 per planner) INVALID_GOAL is returned by a planner.

std::mutex invalidStartOrGoalLock_

Mutex for invalidStartStateCount_, invalidGoalCount_.

bool shortcut_ = {true}

Flag indicating whether to shortcut paths.

bool hybridize_ = {true}

Flag indicating whether to hybridize the set of solution paths.

unsigned int maxHybridPaths_ = {24}

The maximum number of paths that will be hybridized. This prohibits hybridization of a very large path set, which may take significant time.

unsigned int defaultNumPlanners_

The number of planners to use if none are specified. This defaults to the number of cores. This parameter has no effect if planners have already been added.

base::Cost bestCost_ = {std::numeric_limits<double>::quiet_NaN()}

Best cost found so far by algorithm.

std::mutex lock_

mutex for updating bestCost_