Implementation of BundleSpace Rapidly-Exploring Random Trees Algorithm. More...

#include <ompl/multilevel/planners/qrrt/QRRTImpl.h>

Inheritance diagram for ompl::multilevel::QRRTImpl:

[legend]

Public Member Functions
	QRRTImpl (const ompl::base::SpaceInformationPtr &si, BundleSpace *parent_)

virtual void	grow () override
	One iteration of RRT with adjusted sampling function.

Public Member Functions inherited from ompl::multilevel::BundleSpaceGraph
	BundleSpaceGraph (const ompl::base::SpaceInformationPtr &si, BundleSpace *parent=nullptr)

virtual unsigned int	getNumberOfVertices () const

virtual unsigned int	getNumberOfEdges () const

Vertex	nullVertex () const

void	sampleFromDatastructure (ompl::base::State *) override

virtual void	sampleBundleGoalBias (ompl::base::State *xRandom)

bool	getSolution (ompl::base::PathPtr &solution) override
	Return best solution.

virtual ompl::base::PathPtr &	getSolutionPathByReference ()

void	getPlannerData (ompl::base::PlannerData &data) const override
	Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (start/goal/non-connected)

void	getPlannerDataGraph (ompl::base::PlannerData &data, const Graph &graph, const Vertex vStart) const

double	getImportance () const override
	Importance of Bundle-space depending on number of vertices in Bundle-graph.

virtual void	init ()
	Initialization methods for the first iteration (adding start configuration and doing sanity checks)

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.

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

virtual void	clearVertices ()

virtual void	deleteConfiguration (Configuration *q)

template<template< typename T > class NN>
void	setNearestNeighbors ()

void	uniteComponents (Vertex m1, Vertex m2)

bool	sameComponent (Vertex m1, Vertex m2)

virtual const Configuration *	nearest (const Configuration *s) const

void	setMetric (const std::string &sMetric) override

void	setPropagator (const std::string &sPropagator) override

virtual void	setImportance (const std::string &sImportance)

virtual void	setGraphSampler (const std::string &sGraphSampler)

virtual void	setFindSectionStrategy (FindSectionType type)

BundleSpaceGraphSamplerPtr	getGraphSampler ()

virtual Graph &	getGraphNonConst ()
	Get underlying boost graph representation (non const)

virtual const Graph &	getGraph () const
	Get underlying boost graph representation.

const RoadmapNeighborsPtr &	getRoadmapNeighborsPtr () const

void	print (std::ostream &out) const override
	Print class to ostream.

void	writeToGraphviz (std::string filename) const
	Write class to graphviz.

virtual void	printConfiguration (const Configuration *) const
	Print configuration to std::cout.

void	setGoalBias (double goalBias)

double	getGoalBias () const

void	setRange (double distance)

double	getRange () const

ompl::base::PathPtr	getPath (const Vertex &start, const Vertex &goal)
	Shortest path on Bundle-graph.

ompl::base::PathPtr	getPath (const Vertex &start, const Vertex &goal, Graph &graph)

virtual double	distance (const Configuration a, const Configuration b) const
	Distance between two configurations using the current metric.

virtual bool	checkMotion (const Configuration a, const Configuration b) const
	Check if we can move from configuration a to configuration b using the current metric.

bool	connect (const Configuration from, const Configuration to)
	Try to connect configuration a to configuration b using the current metric.

Configuration *	steerTowards (const Configuration from, const Configuration to)
	Steer system at Configuration from to Configuration to.

Configuration *	steerTowards_Range (const Configuration from, Configuration to)
	Steer system at Configuration from to Configuration to, stopping if maxdistance is reached.

Configuration *	extendGraphTowards_Range (const Configuration from, Configuration to)
	Steer system at Configuration from to Configuration to while system is valid, stopping if maxDistance is reached.

virtual void	interpolate (const Configuration a, const Configuration b, Configuration *dest) const
	Interpolate from configuration a to configuration b and store results in dest.

virtual Configuration *	addBundleConfiguration (base::State *)
	Add ompl::base::State to graph. Return its configuration.

virtual Vertex	addConfiguration (Configuration *q)
	Add configuration to graph. Return its vertex in boost graph.

void	addGoalConfiguration (Configuration *x)
	Add configuration to graph as goal vertex.

virtual void	addBundleEdge (const Configuration a, const Configuration b)
	Add edge between configuration a and configuration b to graph.

virtual const std::pair< Edge, bool >	addEdge (const Vertex a, const Vertex b)
	Add edge between Vertex a and Vertex b to graph.

virtual Vertex	getStartIndex () const
	Get vertex representing the start.

virtual Vertex	getGoalIndex () const
	Get vertex representing the goal.

virtual void	setStartIndex (Vertex)
	Set vertex representing the start.

bool	findSection () override
	Call algorithm to solve the find section problem.

Public Member Functions inherited from ompl::multilevel::BundleSpace
	BundleSpace (const ompl::base::SpaceInformationPtr &si, BundleSpace *baseSpace_=nullptr)
	Bundle Space contains three primary characters, the bundle space, the base space and the projection. More...

const ompl::base::SpaceInformationPtr &	getBundle () const
	Get SpaceInformationPtr for Bundle.

const ompl::base::SpaceInformationPtr &	getBase () const
	Get SpaceInformationPtr for Base.

ProjectionPtr	getProjection () const
	Get ProjectionPtr from Bundle to Base.

bool	makeProjection ()
	Given bundle space and base space, try to guess the right.

void	setProjection (ProjectionPtr projection)
	Set explicit projection (so that we do not need to guess.

virtual void	setProblemDefinition (const ompl::base::ProblemDefinitionPtr &pdef) override
	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().

virtual void	sampleBundle (ompl::base::State *xRandom)

bool	sampleBundleValid (ompl::base::State *xRandom)

virtual bool	hasSolution ()

virtual bool	isInfeasible ()
	Check if any infeasibility guarantees are fulfilled.

virtual bool	hasConverged ()
	Check if the current space can still be sampled.

ompl::base::State *	allocIdentityStateBundle () const
	Allocate State, set entries to Identity/Zero.

ompl::base::State *	allocIdentityStateBase () const

ompl::base::State *	allocIdentityState (ompl::base::StateSpacePtr) const

void	allocIdentityState (ompl::base::State *, ompl::base::StateSpacePtr) const

unsigned int	getBaseDimension () const
	Dimension of Base Space.

unsigned int	getBundleDimension () const
	Dimension of Bundle Space.

unsigned int	getCoDimension () const
	Dimension of Bundle Space - Dimension of Base Space.

const ompl::base::StateSamplerPtr &	getBundleSamplerPtr () const

const ompl::base::StateSamplerPtr &	getBaseSamplerPtr () const

BundleSpace *	getChild () const
	Return k-1 th bundle space (locally the base space)

void	setChild (BundleSpace *child)
	Pointer to k-1 th bundle space (locally the base space)

BundleSpace *	getParent () const
	Return k+1 th bundle space (locally the total space)

void	setParent (BundleSpace *parent)
	Pointer to k+1 th bundle space (locally the total space)

bool	hasParent () const
	Return if has parent space pointer.

bool	hasBaseSpace () const
	Return if has base space pointer.

unsigned int	getLevel () const
	Level in hierarchy of Bundle-spaces.

void	setLevel (unsigned int)
	Change level in hierarchy.

void	project (const ompl::base::State xBundle, ompl::base::State xBase) const
	Bundle Space Projection Operator onto first component ProjectBase: Bundle \rightarrow Base.

void	lift (const ompl::base::State xBase, ompl::base::State xBundle) const
	Lift a state from Base to Bundle.

ompl::base::OptimizationObjectivePtr	getOptimizationObjectivePtr () const

bool	isDynamic () const

base::GoalSampleableRegion *	getGoalPtr () const

Public Member Functions inherited from ompl::base::Planner
	Planner (const Planner &)=delete

Planner &	operator= (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 SpaceInformationPtr &	getSpaceInformation () const
	Get the space information this planner is using.

const ProblemDefinitionPtr &	getProblemDefinition () const
	Get the problem definition the planner is trying to solve.

ProblemDefinitionPtr &	getProblemDefinition ()
	Get the problem definition the planner is trying to solve.

const PlannerInputStates &	getPlannerInputStates () const
	Get the planner input states.

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.

virtual void	clearQuery ()
	Clears internal datastructures of any query-specific information from the previous query. Planner settings are not affected. The planner, if able, should retain all datastructures generated from previous queries that can be used to help solve the next query. Note that clear() should also clear all query-specific information along with all other datastructures in the planner. By default clearQuery() calls clear().

const std::string &	getName () const
	Get the name of the planner.

void	setName (const std::string &name)
	Set the name of the planner.

const PlannerSpecs &	getSpecs () 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.

ParamSet &	params ()
	Get the parameters for this planner.

const ParamSet &	params () const
	Get the parameters for this planner.

const PlannerProgressProperties &	getPlannerProgressProperties () 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.

Additional Inherited Members
Public Types inherited from ompl::multilevel::BundleSpaceGraph
using	normalized_index_type = int

using	Graph = boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphMetaData >
	A Bundle-graph structure using boost::adjacency_list bundles. More...

using	BGT = boost::graph_traits< Graph >

using	Vertex = BGT::vertex_descriptor

using	Edge = BGT::edge_descriptor

using	VertexIndex = BGT::vertices_size_type

using	IEIterator = BGT::in_edge_iterator

using	OEIterator = BGT::out_edge_iterator

using	VertexParent = Vertex

using	VertexRank = VertexIndex

using	RoadmapNeighborsPtr = std::shared_ptr< NearestNeighbors< Configuration * > >

using	PDF = ompl::PDF< Configuration * >

using	PDF_Element = PDF::Element

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.

Static Public Member Functions inherited from ompl::multilevel::BundleSpace
static void	resetCounter ()
	reset counter for number of levels

Public Attributes inherited from ompl::multilevel::BundleSpaceGraph
std::map< Vertex, VertexRank >	vrank

std::map< Vertex, Vertex >	vparent

boost::disjoint_sets< boost::associative_property_map< std::map< Vertex, VertexRank > >, boost::associative_property_map< std::map< Vertex, Vertex > > >	disjointSets_ {boost::make_assoc_property_map(vrank), boost::make_assoc_property_map(vparent)}

base::Cost	bestCost_ {+base::dInf}
	Best cost found so far by algorithm.

std::vector< Vertex >	shortestVertexPath_

Protected Types inherited from ompl::multilevel::BundleSpaceGraph
using	RNGType = boost::minstd_rand

Protected Member Functions inherited from ompl::multilevel::BundleSpaceGraph
ompl::base::Cost	costHeuristic (Vertex u, Vertex v) const

Protected Member Functions inherited from ompl::multilevel::BundleSpace
void	checkBundleSpaceMeasure (std::string name, const ompl::base::StateSpacePtr space) const
	Check if Bundle-space is bounded.

void	sanityChecks () const
	Check if Bundle-space has correct structure.

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.

Protected Attributes inherited from ompl::multilevel::BundleSpaceGraph
ompl::base::PathPtr	solutionPath_

Vertex	vStart_

RoadmapNeighborsPtr	nearestDatastructure_
	Nearest neighbor structure for Bundle space configurations.

Graph	graph_

unsigned int	numVerticesWhenComputingSolutionPath_ {0}

RNG	rng_

RNGType	rng_boost

double	graphLength_ {0.0}
	Length of graph (useful for determing importance of Bundle-space.

double	maxDistance_ {-1.0}
	Maximum expansion step.

double	goalBias_ {.1}
	Goal bias.

Configuration *	xRandom_ {nullptr}
	Temporary random configuration.

BundleSpaceImportancePtr	importanceCalculator_ {nullptr}
	Pointer to strategy to compute importance of this bundle space (which is used to decide which bundle space to grow next)

BundleSpaceGraphSamplerPtr	graphSampler_ {nullptr}
	Pointer to strategy to sample from graph.

PathRestrictionPtr	pathRestriction_ {nullptr}
	Pointer to current path restriction (the set of points which project onto the best cost path on the base space if any). This only exists if there exists a base space and there exists a base space path.

ompl::geometric::PathSimplifierPtr	optimizer_
	A path optimizer.

Configuration *	qStart_ {nullptr}
	Start configuration.

Configuration *	qGoal_ {nullptr}
	The (best) goal configuration.

std::vector< Configuration * >	startConfigurations_
	List of configurations that satisfy the start condition.

std::vector< Configuration * >	goalConfigurations_
	List of configurations that satisfy the goal condition.

Protected Attributes inherited from ompl::multilevel::BundleSpace
ompl::base::State *	xBaseTmp_ {nullptr}
	A temporary state on Base.

ompl::base::State *	xBundleTmp_ {nullptr}
	A temporary state on Bundle.

unsigned int	id_ {0}
	Identity of space (to keep track of number of Bundle-spaces created)

bool	hasSolution_ {false}
	If there exists a solution.

bool	firstRun_ {true}
	Variable to check if this bundle space planner has been run at.

bool	isDynamic_ {false}
	If the problem is dynamic or geometric.

BundleSpaceMetricPtr	metric_
	Metric on bundle space.

BundleSpacePropagatorPtr	propagator_
	Propagator (steering or interpolation) on bundle space. Note: currently just a stub for base::StatePropagator.

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.

Static Protected Attributes inherited from ompl::multilevel::BundleSpace
static unsigned int	counter_ = 0
	Internal counter to track multiple bundle spaces.

Detailed Description

Implementation of BundleSpace Rapidly-Exploring Random Trees Algorithm.

Definition at line 85 of file QRRTImpl.h.

The documentation for this class was generated from the following files:

ompl/multilevel/planners/qrrt/QRRTImpl.h
ompl/multilevel/planners/qrrt/src/QRRTImpl.cpp

Public Member Functions

Additional Inherited Members

Detailed Description