Class Syclop

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Derived Types

Class Documentation

class Syclop : public ompl::base::Planner

Synergistic Combination of Layers of Planning.

Short description

Syclop is a multi-layered planner that guides a low-level sampling-based tree planner through a sequence of sequence of discrete workspace regions from start to goal. Syclop is defined as an abstract base class whose pure virtual methods are defined by the chosen low-level sampling-based tree planner.

External documentation

E. Plaku, L.E. Kavraki, and M.Y. Vardi, Motion Planning with Dynamics by a Synergistic Combination of Layers of Planning, in IEEE Transactions on Robotics, 2010. DOI: 10.1109/TRO.2010.2047820

Subclassed by ompl::control::SyclopEST, ompl::control::SyclopRRT

ompl::base::Planner Interface

virtual void setup() override

Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceInformation::setup() if needed. This must be called before solving.

virtual void clear() override

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

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

Continues solving until a solution is found or a given planner termination condition is met. Returns true if solution was found.

Tunable parameters

void setLeadComputeFn(const LeadComputeFn &compute)

Allows the user to override the lead computation function.

void addEdgeCostFactor(const EdgeCostFactorFn &factor)

Adds an edge cost factor to be used for edge weights between adjacent regions.

void clearEdgeCostFactors()

Clears all edge cost factors, making all edge weights equivalent to 1.

inline int getNumFreeVolumeSamples() const

Get the number of states to sample when estimating free volume in the Decomposition.

inline void setNumFreeVolumeSamples(int numSamples)

Set the number of states to sample when estimating free volume in the Decomposition.

inline double getProbShortestPathLead() const

Get the probability [0,1] that a lead will be computed as a shortest-path instead of a random-DFS.

inline void setProbShortestPathLead(double probability)

Set the probability [0,1] that a lead will be computed as a shortest-path instead of a random-DFS.

inline double getProbAddingToAvailableRegions() const

Get the probability [0,1] that the set of available regions will be augmented.

inline void setProbAddingToAvailableRegions(double probability)

Set the probability [0,1] that the set of available regions will be augmented.

inline int getNumRegionExpansions() const

Get the number of times a new region will be chosen and promoted for expansion from a given lead.

inline void setNumRegionExpansions(int regionExpansions)

Set the number of times a new region will be chosen and promoted for expansion from a given lead.

inline int getNumTreeExpansions() const

Get the number of calls to selectAndExtend() in the low-level tree planner for a given lead and region.

inline void setNumTreeExpansions(int treeExpansions)

Set the number of calls to selectAndExtend() in the low-level tree planner for a given lead and region.

inline double getProbAbandonLeadEarly() const

Get the probability [0,1] that a lead will be abandoned early, before a new region is chosen for expansion.

inline void setProbAbandonLeadEarly(double probability)

The probability that a lead will be abandoned early, before a new region is chosen for expansion.

Public Types

using EdgeCostFactorFn = std::function<double(int, int)>

Each edge weight between two adjacent regions in the Decomposition is defined as a product of edge cost factors. By default, given adjacent regions \(r\) and \(s\), Syclop uses the sole edge cost factor.

\[ \frac{1 + \mbox{sel}^2(r,s)}{1 + \mbox{conn}^2(r,s)} \alpha(r) \alpha(s), \]
where for any region \(t\),
\[ \alpha(t) = \frac{1}{\left(1 + \mbox{cov}(t)\right) \mbox{freeVol}^4(t)}, \]
\(\mbox{sel}(r,s)\) is the number of times \(r\) and \(s\) have been part of a lead or selected for exploration, \(\mbox{conn}(r,s)\) estimates the progress made by the low-level planner in extending the tree from \(r\) to \(s\), \(\mbox{cov}(t)\) estimates the tree coverage of the region \(t\), and \(\mbox{freeVol}(t)\) estimates the free volume of \(t\). Additional edge cost factors can be added with the addEdgeCostFactor() function, and Syclop’s list of edge cost factors can be cleared using clearEdgeCostFactors() .

using LeadComputeFn = std::function<void(int, int, std::vector<int>&)>

Leads should consist of a path of adjacent regions in the decomposition that start with the start region and end at the end region. Default is \(A^\ast\) search.

Public Functions

inline Syclop(const SpaceInformationPtr &si, DecompositionPtr d, const std::string &plannerName)

Constructor. Requires a Decomposition, which Syclop uses to create high-level leads.

~Syclop() override = default

Protected Functions

virtual Motion *addRoot(const base::State *s) = 0

Add State s as a new root in the low-level tree, and return the Motion corresponding to s.

virtual void selectAndExtend(Region &region, std::vector<Motion*> &newMotions) = 0

Select a Motion from the given Region, and extend the tree from the Motion. Add any new motions created to newMotions.

inline const Region &getRegionFromIndex(const int rid) const

Returns a reference to the Region object with the given index. Assumes the index is valid.

Protected Attributes

int numFreeVolSamples_ = {Defaults::NUM_FREEVOL_SAMPLES}

The number of states to sample to estimate free volume in the Decomposition.

double probShortestPath_ = {Defaults::PROB_SHORTEST_PATH}

The probability that a lead will be computed as a shortest-path instead of a random-DFS.

double probKeepAddingToAvail_ = {Defaults::PROB_KEEP_ADDING_TO_AVAIL}

The probability that the set of available regions will be augmented.

int numRegionExpansions_ = {Defaults::NUM_REGION_EXPANSIONS}

The number of times a new region will be chosen and promoted for expansion from a given lead.

int numTreeSelections_ = {Defaults::NUM_TREE_SELECTIONS}

The number of calls to selectAndExtend() in the low-level tree planner for a given lead and region.

double probAbandonLeadEarly_ = {Defaults::PROB_ABANDON_LEAD_EARLY}

The probability that a lead will be abandoned early, before a new region is chosen for expansion.

const SpaceInformation *siC_

Handle to the control::SpaceInformation object.

DecompositionPtr decomp_

The high level decomposition used to focus tree expansion.

RNG rng_

Random number generator.

class Adjacency

Representation of an adjacency (a directed edge) between two regions in the Decomposition assigned to Syclop.

Public Functions

Adjacency() = default
virtual ~Adjacency() = default
inline void clear()

Clears coverage information from this adjacency.

Public Members

std::set<int> covGridCells = {}

The cells of the underlying coverage grid that contain tree motions originating from direct connections along this adjacency.

const Region *source = {nullptr}

The source region of this adjacency edge.

const Region *target = {nullptr}

The target region of this adjacency edge.

double cost = {0.}

The cost of this adjacency edge, used in lead computations.

int numLeadInclusions = {0}

The number of times this adjacency has been included in a lead.

int numSelections = {0}

The number of times the low-level tree planner has selected motions from the source region when attempting to extend the tree toward the target region.

bool empty = {false}

This value is true if and only if this adjacency’s source and target regions both contain zero tree motions.

struct Defaults

Contains default values for Syclop parameters.

Public Static Attributes

static const int NUM_FREEVOL_SAMPLES = 100000
static const int COVGRID_LENGTH = 128
static const int NUM_REGION_EXPANSIONS = 100
static const int NUM_TREE_SELECTIONS = 1
static const double PROB_ABANDON_LEAD_EARLY
static const double PROB_KEEP_ADDING_TO_AVAIL
static const double PROB_SHORTEST_PATH
class Motion

Representation of a motion.

A motion contains pointers to its state, its parent motion, and the control that was applied to get from its parent to its state.

Public Functions

Motion() = default
inline Motion(const SpaceInformation *si)

Constructor that allocates memory for the state and the control.

virtual ~Motion() = default

Public Members

base::State *state = {nullptr}

The state contained by the motion.

Control *control = {nullptr}

The control contained by the motion.

const Motion *parent = {nullptr}

The parent motion in the tree.

unsigned int steps = {0}

The number of steps for which the control is applied.

class Region

Representation of a region in the Decomposition assigned to Syclop.

Public Functions

Region() = default
virtual ~Region() = default
Region(const Region&) = default
Region &operator=(const Region&) = default
Region(Region&&) = default
Region &operator=(Region&&) = default
inline void clear()

Clears motions and coverage information from this region.

Public Members

std::set<int> covGridCells

The cells of the underlying coverage grid that contain tree motions from this region.

std::vector<Motion*> motions

The tree motions contained in this region.

double volume

The volume of this region.

double freeVolume

The free volume of this region.

double percentValidCells

The percent of free volume of this region.

double weight

The probabilistic weight of this region, used when sampling from PDF.

double alpha

The coefficient contributed by this region to edge weights in lead computations.

int index

The index of the graph node corresponding to this region.

unsigned int numSelections

The number of times this region has been selected for expansion.

PDF<int>::Element *pdfElem

The Element corresponding to this region in the PDF of available regions.