Public Member Functions
Construction
	BasicRRTStrategy ()

	BasicRRTStrategy (XMLNode &_node)

virtual	~BasicRRTStrategy ()=default

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
Motion Planning Types
typedef MPTraits::CfgType	CfgType

typedef MPTraits::WeightType	WeightType

typedef MPTraits::RoadmapType	RoadmapType

typedef RoadmapType::VID	VID

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

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 BasicRRTStrategy< MPTraits >

The RRT algorithm grows one or more trees from a set of root nodes to solve a single-query planning problem.

References: Original RRT: LaValle, Steven M. "Rapidly-Exploring Random Trees: A New Tool for Path Planning." TR 98-11, Computer Science Dept., Iowa State Univ., 1998. RRT Connect (bi-directional): James Kuffner and Steven LaValle. "RRT-Connect: An Efficient Approach to Single-Query Path Planning". ICRA 2000. Nonholonomic RRT: Steven LaValle and James Kuffner. "Randomized Kinodynamic Planning." IJRR 2001.

This method supports both uni-directional and bi-directional variants. Nonholonomic problems are supported with the appropriate extender and uni-directional growth.

For uni-directional methods, we support an additional 'goal extension' heuristic which attempts to connect configurations near the goal to the goal region. This is necessary to get RRT to terminate - the alternative is to rely on goal-biased sampling to eventually complete the problem, which is not an efficient solution.

For bi-directional methods, the algorithm will attempt to connect trees after each successful extension. The new node will be extended toward the nearest neighbor in each other tree. If the extension reaches its destination, the two trees will merge.

Member Function Documentation

◆ Print()

template<typename MPTraits >

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

virtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from MPStrategyMethod< MPTraits >.

Reimplemented in AdaptiveRRT< MPTraits >.

◆ Initialize()

template<typename MPTraits >

void BasicRRTStrategy< 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 MPBaseObject< MPTraits >.

Reimplemented in AdaptiveRRT< MPTraits >.

◆ Iterate()

template<typename MPTraits >

void BasicRRTStrategy< MPTraits >::Iterate

overrideprotectedvirtual

Execute one iteration of the strategy.

Reimplemented from MPStrategyMethod< MPTraits >.

◆ SelectDispersedTarget()

template<typename MPTraits >

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

protected

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`
	)

protectedvirtual

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
	)

protectedvirtual

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`
	)

protectedvirtual

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`
	)

protected

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

◆ AddNode()

template<typename MPTraits >

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

protectedvirtual

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
	)

protectedvirtual

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 )

protected

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 )

protected

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
	)

protected

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() [1/2]

template<typename MPTraits >

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

protected

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() [2/2]

template<typename MPTraits >

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

protectedvirtual

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 )

protected

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:

BasicRRTStrategy.h

Public Member Functions

Public Types

Protected Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

◆ Print()

◆ Initialize()

◆ Iterate()

◆ SelectDispersedTarget()

◆ FindNearestNeighbor()

◆ SelectNeighbor()

◆ Extend() [1/2]

◆ Extend() [2/2]

◆ AddNode()

◆ AddEdge()

◆ ConnectNeighbors()

◆ TryGoalExtension() [1/2]

◆ TryGoalExtension() [2/2]

◆ ExpandTree() [1/2]

◆ ExpandTree() [2/2]

◆ ConnectTrees()

◆ EnableOutputFiles()

◆ Run()

◆ Finalize()

◆ GenerateStart()

◆ GenerateGoals()

◆ SetName()

◆ GetBaseFilename()