Public Member Functions

	AdaptiveRRT (double _wallPenalty=0.5, double _gamma=0.5, const GrowthSets &_growthSets=GrowthSets(), CostMethod _c=FIXED)

	AdaptiveRRT (XMLNode &_node)

MPBaseObject Overrides
virtual void	Print (std::ostream &_os) const

Interface
void	operator() ()
	Execute the strategy by calling Initialize, Run, and Finalize.

void	EnableOutputFiles (const bool _enable=true)

Name and Label Accessors
const std::string &	GetName () const
	Get the class name for this object.

const std::string &	GetLabel () const
	Get the unique label for this object.

std::string	GetNameAndLabel () const
	Get the unique string identifier for this object "m_name::m_label".

void	SetLabel (const std::string &)
	Set the unique label for this object.

MPLibrary Accessors
void	SetMPLibrary (MPLibrary *) noexcept
	Set the owning MPLibrary.

MPLibrary *	GetMPLibrary () const noexcept
	Get the owning MPLibrary.

bool	IsRunning () const noexcept
	Check the library's running flag.

DistanceMetricPointer	GetDistanceMetric (const std::string &) const noexcept
	Get a distance metric by label from the owning MPLibrary.

ValidityCheckerPointer	GetValidityChecker (const std::string &) const noexcept
	Get a validity checker by label from the owning MPLibrary.

NeighborhoodFinderPointer	GetNeighborhoodFinder (const std::string &) const noexcept
	Get a neighborhood finder by label from the owning MPLibrary.

SamplerPointer	GetSampler (const std::string &) const noexcept
	Get a sampler by label from the owning MPLibrary.

LocalPlannerPointer	GetLocalPlanner (const std::string &) const noexcept
	Get a local planner by label from the owning MPLibrary.

ExtenderPointer	GetExtender (const std::string &) const noexcept
	Get an extender by label from the owning MPLibrary.

PathModifierPointer	GetPathModifier (const std::string &) const noexcept
	Get a path modifier by label from the owning MPLibrary.

ConnectorPointer	GetConnector (const std::string &) const noexcept
	Get a connector by label from the owning MPLibrary.

MetricPointer	GetMetric (const std::string &) const noexcept
	Get a metric by label from the owning MPLibrary.

MapEvaluatorPointer	GetMapEvaluator (const std::string &) const noexcept
	Get a map evaluator by label from the owning MPLibrary.

MPStrategyPointer	GetMPStrategy (const std::string &) const noexcept
	Get a strategy by label from the owning MPLibrary.

MPTraits::MPTools *	GetMPTools () const noexcept
	Get the MPTools container from the owning MPLibrary.

Problem Accessors
MPProblem *	GetMPProblem () const noexcept
	Get the library's current MPProblem.

Environment *	GetEnvironment () const noexcept
	Get the current environment.

MPTask *	GetTask () const noexcept
	Get the current task.

GroupTask *	GetGroupTask () const noexcept
	Get the current group task.

Solution Accessors
MPSolution *	GetMPSolution () const noexcept

RoadmapType *	GetRoadmap (Robot *const _r=nullptr) const noexcept
	Get the current free-space roadmap.

GroupRoadmapType *	GetGroupRoadmap (RobotGroup *const _g=nullptr) const noexcept
	Get the current free-space group roadmap.

RoadmapType *	GetBlockRoadmap (Robot *const _r=nullptr) const noexcept
	Get the current obstacle-space roadmap.

Path *	GetPath (Robot *const _r=nullptr) const noexcept
	Get the current best path.

GroupPath *	GetGroupPath (RobotGroup *const _g=nullptr) const noexcept
	Get the current best group path.

StatClass *	GetStatClass () const noexcept
	Get the current StatClass.

LocalObstacleMap *	GetLocalObstacleMap () const noexcept
	Get the local obstacle map.

GoalTracker *	GetGoalTracker () const noexcept
	Get the goal tracker.

Public Types
Local Types
enum	CostMethod { FIXED , REWARD , CYCLES }

Motion Planning Types
typedef std::map< std::string, std::pair< std::pair< double, long >, double > >	GrowthSet

typedef std::map< double, GrowthSet >	GrowthSets

typedef MPTraits::CfgType	CfgType

typedef MPTraits::WeightType	WeightType

typedef MPTraits::RoadmapType	RoadmapType

typedef RoadmapType::VID	VID

Motion Planning Types
typedef RoadmapType::VertexSet	VertexSet

Local Types
typedef MPTraits::GroupRoadmapType	GroupRoadmapType

typedef MPTraits::LocalObstacleMap	LocalObstacleMap

typedef MPTraits::Path	Path

typedef MPTraits::GroupPathType	GroupPath

typedef MPTraits::MPLibrary	MPLibrary

typedef MPTraits::MPSolution	MPSolution

typedef MPTraits::GoalTracker	GoalTracker

typedef MPLibrary::SamplerPointer	SamplerPointer

typedef MPLibrary::LocalPlannerPointer	LocalPlannerPointer

typedef MPLibrary::ExtenderPointer	ExtenderPointer

typedef MPLibrary::PathModifierPointer	PathModifierPointer

typedef MPLibrary::ConnectorPointer	ConnectorPointer

typedef MPLibrary::MetricPointer	MetricPointer

typedef MPLibrary::MapEvaluatorPointer	MapEvaluatorPointer

typedef MPLibrary::MPStrategyPointer	MPStrategyPointer

typedef MPLibrary::DistanceMetricPointer	DistanceMetricPointer

typedef MPLibrary::ValidityCheckerPointer	ValidityCheckerPointer

typedef MPLibrary::NeighborhoodFinderPointer	NeighborhoodFinderPointer

Protected Member Functions
void	SetName (const std::string &_s)

const std::string &	GetBaseFilename () const

MPStrategy Overrides
virtual void	Initialize () override

Tree Helpers
virtual VID	ExpandTree (CfgType &_dir)

std::string	SelectGrowthMethod (GrowthSet &_gs)

void	UpdateCost (double _cost, std::string _s, GrowthSet &_gs)

void	RewardGrowthMethod (double _r, std::string _s, GrowthSet &_gs)

VID	UpdateTree (VID _nearest, CfgType &_new, CfgType &_dir, double _delta)

VID	AddNode (CfgType &_newCfg, VID _nearVID, bool _againstWall, double _ratio)

MPStrategy Overrides
virtual void	Iterate () override
	Execute one iteration of the strategy. More...

Direction Helpers
virtual CfgType	SelectTarget ()
	Get a random configuration to grow towards.

CfgType	SelectDispersedTarget (const VID _v)

Neighbor Helpers
virtual VID	FindNearestNeighbor (const CfgType &_cfg, const VertexSet *const _candidates=nullptr)

virtual Neighbor	SelectNeighbor (const CfgType &_cfg, const std::vector< Neighbor > &_neighbors)

Growth Helpers
virtual VID	Extend (const VID _nearVID, const CfgType &_target, LPOutput< MPTraits > &_lp, const bool _requireNew=true)

VID	Extend (const VID _nearVID, const CfgType &_target, const bool _requireNew=true)

virtual std::pair< VID, bool >	AddNode (const CfgType &_newCfg)

virtual void	AddEdge (const VID _source, const VID _target, const LPOutput< MPTraits > &_lpOutput)

void	ConnectNeighbors (const VID _newVID)

void	TryGoalExtension (const VID _newVID)

void	TryGoalExtension (const VID _newVID, const Boundary *const _boundary)

Tree Helpers
VID	ExpandTree (const CfgType &_target)

virtual VID	ExpandTree (const VID _nearestVID, const CfgType &_target)

void	ConnectTrees (const VID _recentlyGrown)

Helpers
virtual void	Run ()
	Call Iterate until EvaluateMap is true. More...

virtual bool	EvaluateMap ()
	Check if we satisfied all map evaluators.

virtual void	Finalize ()
	Clean-up and output results. More...

virtual void	ClearRoadmap ()
	Pre-clear the roadmap(s) if requested.

Start/Goal Generation
virtual size_t	GenerateStart (const std::string &_samplerLabel="")

virtual std::vector< size_t >	GenerateGoals (const std::string &_samplerLabel="")

Protected Attributes
bool	m_debug
	Print debug info?

MP Object Labels
std::string	m_samplerLabel
	The sampler label.

std::string	m_nfLabel
	The neighborhood finder label.

std::string	m_ncLabel
	The connector label (for RRG).

std::string	m_exLabel
	The extender label.

std::string	m_goalDmLabel
	Dm for checking goal extensions.

std::string	m_fallbackNfLabel
	NF for searching the active set, used if the main one fails.

RRT Properties
bool	m_growGoals {false}
	Grow trees from goals.

double	m_growthFocus {0}
	The fraction of goal-biased expansions.

double	m_goalThreshold {0}
	Distance threshold for goal extension.

size_t	m_numDirections {1}
	Expansion directions per iteration.

size_t	m_disperseTrials {3}
	Sample attempts for disperse search.

Internal State
std::vector< VertexSet >	m_trees
	The current tree set.

Internal State
std::string	m_querySampler
	Sampler for generating start/goal.

std::vector< std::string >	m_meLabels
	The list of map evaluators to use.

size_t	m_iterations {0}
	The number of executed iterations.

bool	m_writeOutput {true}
	Write output at the end?

bool	m_clearMap {false}
	Clear the roadmap(s) before run?

Detailed Description

template<typename MPTraits>
class AdaptiveRRT< MPTraits >

Adaptively selects growth methods in RRT.

References: Denny, Jory, et al. "Adapting RRT growth for heterogeneous environments." 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013.

AdaptiveRRT employs structural filtering to the RRT paradigm by providing a two-level cost-adaptive strategy to select the RRT growth method. First, it uses the "visibility" of a node the method selects a set of RRT methods to choose from based on some probability distribution. This probability distribution is updated based upon the success/fail of the growth and its cost.

Constructor & Destructor Documentation

◆ AdaptiveRRT()

template<typename MPTraits >

AdaptiveRRT< MPTraits >::AdaptiveRRT	(	double	_wallPenalty = `0.5`,
		double	_gamma = `0.5`,
		const GrowthSets &	_growthSets = `GrowthSets()`,
		CostMethod	_c = `FIXED`
	)

@ Construction

Member Function Documentation

◆ Print()

template<typename MPTraits >

void AdaptiveRRT< MPTraits >::Print ( std::ostream & _os ) const

virtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from BasicRRTStrategy< MPTraits >.

◆ Initialize()

template<typename MPTraits >

void AdaptiveRRT< MPTraits >::Initialize

overrideprotectedvirtual

Initialize this object for the current MPProblem. This should reset any internal state of the algorithms so that they are ready for execution. It is also the place to initialize any state that depends on the current problem.

Warning: This member will be called for every compiled algorithm in the planning library - even those that will not be used. If an algorithm needs to do expenisve setup, then this method should only set a flag that tells it to do so on first use. The only exceptions are the MPStrategies, which will only have their initialize called on first use.

Reimplemented from BasicRRTStrategy< MPTraits >.

◆ ExpandTree() [1/3]

template<typename MPTraits >

AdaptiveRRT< MPTraits >::VID AdaptiveRRT< MPTraits >::ExpandTree ( CfgType & _dir )

protectedvirtual

Expand tree in the direction of a given configuration

Parameters

_dir	Direction of random configuration

Returns: VID of newly created Cfg if successful, otherwise INVALID_VID

◆ SelectGrowthMethod()

template<typename MPTraits >

std::string AdaptiveRRT< MPTraits >::SelectGrowthMethod ( GrowthSet & _gs )

protected

Select growth method from a set of growth methods

Parameters

_gs	The growth set containing the growth method we want to access

Returns: The string indicator for the chosen growth method

◆ UpdateCost()

template<typename MPTraits >

void AdaptiveRRT< MPTraits >::UpdateCost	(	double	_cost,
		std::string	_s,
		GrowthSet &	_gs
	)

protected

Update average cost of growth method

Parameters

_cost	Average cost used to update cost of growth method
_s	String indicator for growth method
_gs	The growth set containing the growth method we want to access

◆ RewardGrowthMethod()

template<typename MPTraits >

void AdaptiveRRT< MPTraits >::RewardGrowthMethod	(	double	_r,
		std::string	_s,
		GrowthSet &	_gs
	)

protected

Update weight of growth method given a reward value

Parameters

_r	Reward value
_s	String indicator for growth method
_gs	The growth set containing the growth method we want to access

◆ UpdateTree()

template<typename MPTraits >

AdaptiveRRT< MPTraits >::VID AdaptiveRRT< MPTraits >::UpdateTree	(	VID	_nearest,
		CfgType &	_new,
		CfgType &	_dir,
		double	_delta
	)

protected

Updates tree based on expansion type

Parameters

_nearest	Vertex ID of nearest configuration
_new	New configuration to be added to tree
_dir	Direction of random configuration
_delta	Expansion distance between _nearest and _new

Returns: The VID of the newly added configuration

◆ AddNode() [1/2]

template<typename MPTraits >

AdaptiveRRT< MPTraits >::VID AdaptiveRRT< MPTraits >::AddNode	(	CfgType &	_newCfg,
		VID	_nearVID,
		bool	_againstWall,
		double	_ratio
	)

protected

Adds node to tree and updates visibility

Parameters

_newCfg	New configuration to add to tree
_nearVID	Vertex ID of nearest tree configuration
_againstWall	Bool flag indicating if _newCfg is against a wall.
_ratio	Reward ratio

Returns: The VID of the configuration that was added to the tree

◆ Iterate()

template<typename MPTraits >

void BasicRRTStrategy< MPTraits >::Iterate

overrideprotectedvirtualinherited

Execute one iteration of the strategy.

Reimplemented from MPStrategyMethod< MPTraits >.

◆ SelectDispersedTarget()

template<typename MPTraits >

MPTraits::CfgType BasicRRTStrategy< MPTraits >::SelectDispersedTarget ( const VID _v )

protectedinherited

Sample a target configuration to grow towards from an existing configuration. m_disperseTrials samples are attempted.

Parameters

_v	The VID of the existing configuration.

Returns: The sample who's growth direction yields the greatest separation from the existing configuration's neighbors.

Todo:: This functionality can probably be moved into a dispersed extender, which we could call several times here.

◆ FindNearestNeighbor()

template<typename MPTraits >

BasicRRTStrategy< MPTraits >::VID BasicRRTStrategy< MPTraits >::FindNearestNeighbor	(	const CfgType &	_cfg,
		const VertexSet *const	_candidates = `nullptr`
	)

protectedvirtualinherited

Find the nearest roadmap configuration to an arbitrary configuration.

Parameters

_cfg	The query configuration.
_candidates	The candidate set to search, or null for whole roadmap.

Returns: The VID of the roadmap configuration nearest to _cfg.

◆ SelectNeighbor()

template<typename MPTraits >

Neighbor BasicRRTStrategy< MPTraits >::SelectNeighbor	(	const CfgType &	_cfg,
		const std::vector< Neighbor > &	_neighbors
	)

protectedvirtualinherited

Select the best neighbor from the set of candidates returned by the NF. Default implementation selects the nearest.

Parameters

_cfg	The query configuration.
_neighbors	The set of neighbors returned by the NF.

Returns: The best of _neighbors to extend from for this method.

◆ Extend() [1/2]

template<typename MPTraits >

BasicRRTStrategy< MPTraits >::VID BasicRRTStrategy< MPTraits >::Extend	(	const VID	_nearVID,
		const CfgType &	_target,
		LPOutput< MPTraits > &	_lp,
		const bool	_requireNew = `true`
	)

protectedvirtualinherited

Extend a new configuration from a nearby configuration towards a growth target.

Parameters

_nearVID	The nearby configuration's VID.
_target	The growth target.
_lp	An LPOutput for returning local planner info.
_requireNew	Require the extension to generate a new roadmap node (true unless connecting trees).

Returns: The new node's VID.

◆ Extend() [2/2]

template<typename MPTraits >

BasicRRTStrategy< MPTraits >::VID BasicRRTStrategy< MPTraits >::Extend	(	const VID	_nearVID,
		const CfgType &	_target,
		const bool	_requireNew = `true`
	)

protectedinherited

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

◆ AddNode() [2/2]

template<typename MPTraits >

std::pair< typename BasicRRTStrategy< MPTraits >::VID, bool > BasicRRTStrategy< MPTraits >::AddNode ( const CfgType & _newCfg )

protectedvirtualinherited

Add a new configuration to the roadmap and current tree.

Parameters

_newCfg The new configuration to add.

Returns: A pair with the added VID and a bool indicating whether the new node was already in the map.

◆ AddEdge()

template<typename MPTraits >

void BasicRRTStrategy< MPTraits >::AddEdge	(	const VID	_source,
		const VID	_target,
		const LPOutput< MPTraits > &	_lpOutput
	)

protectedvirtualinherited

Add a new edge to the roadmap.

Parameters

_source	The source node.
_target	The target node.
_lpOutput	The extender output.

◆ ConnectNeighbors()

template<typename MPTraits >

void BasicRRTStrategy< MPTraits >::ConnectNeighbors ( const VID _newVID )

protectedinherited

Try to connect a configuration to its neighbors.

Parameters

_newVID The VID of the configuration to connect.

◆ TryGoalExtension() [1/2]

template<typename MPTraits >

void BasicRRTStrategy< MPTraits >::TryGoalExtension ( const VID _newVID )

protectedinherited

Try to extend a new configuration toward each goal region that is within the extender's range.

Parameters

_newVID The VID of a newly extended configuration.

Note: This only applies when not growing goals.

◆ TryGoalExtension() [2/2]

template<typename MPTraits >

void BasicRRTStrategy< MPTraits >::TryGoalExtension	(	const VID	_newVID,
		const Boundary *const	_boundary
	)

protectedinherited

Try to extend a new configuration toward a specific goal region. No-op if the goal is outside the extender's range.

Parameters

_newVID	The VID of a newly extended configuration.
_boundary	The goal boundary.

Note: This only applies when not growing goals.

◆ ExpandTree() [2/3]

template<typename MPTraits >

BasicRRTStrategy< MPTraits >::VID BasicRRTStrategy< MPTraits >::ExpandTree ( const CfgType & _target )

protectedinherited

Attempt to expand the map by growing towards a target configuration from the nearest existing node.

Parameters

_target The target configuration.

Returns: The VID of a newly created Cfg if successful, INVALID_VID otherwise.

◆ ExpandTree() [3/3]

template<typename MPTraits >

BasicRRTStrategy< MPTraits >::VID BasicRRTStrategy< MPTraits >::ExpandTree	(	const VID	_nearestVID,
		const CfgType &	_target
	)

protectedvirtualinherited

Attempt to expand the map by growing towards a target configuration from an arbitrary existing node.

Parameters

_nearestVID	The VID to grow from.
_target	The target configuration.

Returns: The VID of a newly created Cfg if successful, INVALID_VID otherwise.

◆ ConnectTrees()

template<typename MPTraits >

void BasicRRTStrategy< MPTraits >::ConnectTrees ( const VID _recentlyGrown )

protectedinherited

If multiple trees exist, try to connect the current tree with the one that is nearest to a recently grown configuration.

Parameters

_recentlyGrown The VID of the recently grown configuration.

◆ EnableOutputFiles()

template<typename MPTraits >

void MPStrategyMethod< MPTraits >::EnableOutputFiles ( const bool _enable = true )

inherited

Set output file writing to on or off (on by default). This is used to suppress generation of roadmap, path, and stat files.

Parameters

_enable True to enable, false to disable.

◆ Run()

template<typename MPTraits >

void MPStrategyMethod< MPTraits >::Run

protectedvirtualinherited

Call Iterate until EvaluateMap is true.

Reimplemented in ValidationStrategy< MPTraits >.

◆ Finalize()

template<typename MPTraits >

void MPStrategyMethod< MPTraits >::Finalize

protectedvirtualinherited

Clean-up and output results.

Reimplemented in GroupDecoupledStrategy< MPTraits >, GroupStrategyMethod< MPTraits >, and ValidationStrategy< MPTraits >.

◆ GenerateStart()

template<typename MPTraits >

size_t MPStrategyMethod< MPTraits >::GenerateStart ( const std::string & _samplerLabel = "" )

protectedvirtualinherited

Generate a 'start' node for the task and add it to the roadmap.

Parameters

_samplerLabel The label for the sampler to use if no query sampler was provided, or empty to require a query sampler.

Returns: The VID of the generated configuration.

Note: This returns size_t so that it will work for both individual and group strategies (all VID types are typedefs for size_t).

Reimplemented in GroupStrategyMethod< MPTraits >.

◆ GenerateGoals()

template<typename MPTraits >

std::vector< size_t > MPStrategyMethod< MPTraits >::GenerateGoals ( const std::string & _samplerLabel = "" )

protectedvirtualinherited

Generate 'goal' node(s) for the task and add it(them) to the roadmap.

Parameters

_samplerLabel The label for the sampler to use if no query sampler was provided, or empty to require a query sampler.

Returns: The VIDs of the generated configurations.

Note: This returns size_t so that it will work for both individual and group strategies (all VID types are typedefs for size_t).

Reimplemented in GroupStrategyMethod< MPTraits >.

◆ SetName()

template<typename MPTraits >

void MPBaseObject< MPTraits >::SetName ( const std::string & _s )

protectedinherited

Parameters

_s	Class name

◆ GetBaseFilename()

template<typename MPTraits >

const std::string & MPBaseObject< MPTraits >::GetBaseFilename ( ) const

protectedinherited

Returns: base file name from MPProblem

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

AdaptiveRRT.h

Public Member Functions

Public Types

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ AdaptiveRRT()

Member Function Documentation

◆ Print()

◆ Initialize()

◆ ExpandTree() [1/3]

◆ SelectGrowthMethod()

◆ UpdateCost()

◆ RewardGrowthMethod()

◆ UpdateTree()

◆ AddNode() [1/2]

◆ Iterate()

◆ SelectDispersedTarget()

◆ FindNearestNeighbor()

◆ SelectNeighbor()

◆ Extend() [1/2]

◆ Extend() [2/2]

◆ AddNode() [2/2]

◆ AddEdge()

◆ ConnectNeighbors()

◆ TryGoalExtension() [1/2]

◆ TryGoalExtension() [2/2]

◆ ExpandTree() [2/3]

◆ ExpandTree() [3/3]

◆ ConnectTrees()

◆ EnableOutputFiles()

◆ Run()

◆ Finalize()

◆ GenerateStart()

◆ GenerateGoals()

◆ SetName()

◆ GetBaseFilename()