#include <BasicRRTStrategy.h>

Inheritance diagram for BasicRRTStrategy:

[legend]

Collaboration diagram for BasicRRTStrategy:

[legend]

Public Types
Motion Planning Types
typedef size_t	VID

typedef std::unordered_set< VID >	VertexSet

Public Types inherited from MPStrategyMethod
typedef size_t	VID

Public Types inherited from MPBaseObject
typedef DefaultWeight< Cfg >	WeightType

typedef GenericStateGraph< Cfg, WeightType >	RoadmapType

typedef GroupCfg< RoadmapType >	GroupCfgType

typedef GroupLocalPlan< RoadmapType >	GroupWeightType

typedef GroupRoadmap< GroupCfgType, GroupWeightType >	GroupRoadmapType

Public Member Functions
Construction
	BasicRRTStrategy ()

	BasicRRTStrategy (XMLNode &_node)

virtual	~BasicRRTStrategy ()

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

Public Member Functions inherited from MPStrategyMethod
	MPStrategyMethod ()=default

	MPStrategyMethod (XMLNode &_node)

virtual	~MPStrategyMethod ()

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

void	EnableOutputFiles (const bool _enable=true)

virtual void	Initialize ()

Public Member Functions inherited from MPBaseObject
	MPBaseObject (const std::string &_label="", const std::string &_name="", bool _debug=false)

	MPBaseObject (XMLNode &_node)

virtual	~MPBaseObject ()

const std::string &	GetName () const
	Get the class name for this object. More...

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

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

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

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

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

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

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

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

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

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

MPSolutionType *	GetMPSolution () const noexcept

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

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

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

Path *	GetPath (Robot *const _r=nullptr) const noexcept

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

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

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

Protected Member Functions
MPStrategy Overrides
virtual void	Initialize () override

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

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

Cfg	SelectDispersedTarget (const VID _v)

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

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

Growth Helpers
virtual VID	Extend (const VID _nearVID, const Cfg &_target, LPOutput &_lp, const bool _requireNew=true)

VID	Extend (const VID _nearVID, const Cfg &_target, const bool _requireNew=true)
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More...

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

virtual void	AddEdge (const VID _source, const VID _target, const LPOutput &_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 Cfg &_target)

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

void	ConnectTrees (const VID _recentlyGrown)

Protected Member Functions inherited from MPStrategyMethod
virtual void	Run ()
	Call Iterate until EvaluateMap is true. More...

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

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

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

virtual size_t	GenerateStart (const std::string &_samplerLabel="")

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

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

const std::string &	GetBaseFilename () const

Protected Attributes
MP Object Labels
std::string	m_samplerLabel
	The sampler label. More...

std::string	m_nfLabel
	The neighborhood finder label. More...

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

std::string	m_exLabel
	The extender label. More...

std::string	m_goalDmLabel
	Dm for checking goal extensions. More...

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

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

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

double	m_goalThreshold {0}
	Distance threshold for goal extension. More...

size_t	m_numDirections {1}
	Expansion directions per iteration. More...

size_t	m_disperseTrials {3}
	Sample attempts for disperse search. More...

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

Protected Attributes inherited from MPStrategyMethod
std::string	m_querySampler
	Sampler for generating start/goal. More...

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

size_t	m_iterations {0}
	The number of executed iterations. More...

bool	m_writeOutput {true}
	Write output at the end? More...

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

Protected Attributes inherited from MPBaseObject
bool	m_debug
	Print debug info? More...

Detailed Description

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 Typedef Documentation

◆ VertexSet

typedef std::unordered_set<VID> BasicRRTStrategy::VertexSet

◆ VID

typedef size_t BasicRRTStrategy::VID

Constructor & Destructor Documentation

◆ BasicRRTStrategy() [1/2]

BasicRRTStrategy::BasicRRTStrategy ( )

◆ BasicRRTStrategy() [2/2]

BasicRRTStrategy::BasicRRTStrategy ( XMLNode & _node )

◆ ~BasicRRTStrategy()

BasicRRTStrategy::~BasicRRTStrategy ( )

virtual

Member Function Documentation

◆ AddEdge()

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

protectedvirtual

Add a new edge to the roadmap.

Parameters

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

Reimplemented in Syclop.

◆ AddNode()

std::pair< typename BasicRRTStrategy::VID, bool > BasicRRTStrategy::AddNode ( const Cfg & _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.

Reimplemented in Syclop, and DynamicRegionRRT.

◆ ConnectNeighbors()

void BasicRRTStrategy::ConnectNeighbors ( const VID _newVID )

protected

Try to connect a configuration to its neighbors.

Parameters

_newVID The VID of the configuration to connect.

◆ ConnectTrees()

void BasicRRTStrategy::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.

◆ ExpandTree() [1/2]

BasicRRTStrategy::VID BasicRRTStrategy::ExpandTree ( const Cfg & _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]

BasicRRTStrategy::VID BasicRRTStrategy::ExpandTree	(	const VID	_nearestVID,
		const Cfg &	_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.

Reimplemented in EET, and DynamicDomainRRT.

◆ Extend() [1/2]

BasicRRTStrategy::VID BasicRRTStrategy::Extend	(	const VID	_nearVID,
		const Cfg &	_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.

◆ Extend() [2/2]

BasicRRTStrategy::VID BasicRRTStrategy::Extend	(	const VID	_nearVID,
		const Cfg &	_target,
		LPOutput &	_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.

Reimplemented in Syclop, and DynamicDomainRRT.

◆ FindNearestNeighbor()

BasicRRTStrategy::VID BasicRRTStrategy::FindNearestNeighbor	(	const Cfg &	_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.

Reimplemented in Syclop.

◆ Initialize()

void BasicRRTStrategy::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.

Reimplemented in Syclop, EET, DynamicRegionRRT, DynamicDomainRRT, and AdaptiveRRT.

◆ Iterate()

void BasicRRTStrategy::Iterate ( )

overrideprotectedvirtual

Execute one iteration of the strategy.

Reimplemented from MPStrategyMethod.

Reimplemented in EET.

◆ Print()

void BasicRRTStrategy::Print ( std::ostream & _os ) const

virtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from MPStrategyMethod.

Reimplemented in DynamicRegionRRT, DynamicDomainRRT, and AdaptiveRRT.

◆ SelectDispersedTarget()

Cfg BasicRRTStrategy::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.

◆ SelectNeighbor()

Neighbor BasicRRTStrategy::SelectNeighbor	(	const Cfg &	_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.

◆ SelectTarget()

Cfg BasicRRTStrategy::SelectTarget ( )

protectedvirtual

Get a random configuration to grow towards.

Reimplemented in EET, and DynamicRegionRRT.

◆ TryGoalExtension() [1/2]

void BasicRRTStrategy::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]

void BasicRRTStrategy::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.

Field Documentation

◆ m_disperseTrials

size_t BasicRRTStrategy::m_disperseTrials {3}

protected

Sample attempts for disperse search.

◆ m_exLabel

std::string BasicRRTStrategy::m_exLabel

protected

The extender label.

◆ m_fallbackNfLabel

std::string BasicRRTStrategy::m_fallbackNfLabel

protected

NF for searching the active set, used if the main one fails.

◆ m_goalDmLabel

std::string BasicRRTStrategy::m_goalDmLabel

protected

Dm for checking goal extensions.

◆ m_goalThreshold

double BasicRRTStrategy::m_goalThreshold {0}

protected

Distance threshold for goal extension.

◆ m_growGoals

bool BasicRRTStrategy::m_growGoals {false}

protected

Grow trees from goals.

◆ m_growthFocus

double BasicRRTStrategy::m_growthFocus {0}

protected

The fraction of goal-biased expansions.

◆ m_ncLabel

std::string BasicRRTStrategy::m_ncLabel

protected

The connector label (for RRG).

◆ m_nfLabel

std::string BasicRRTStrategy::m_nfLabel

protected

The neighborhood finder label.

◆ m_numDirections

size_t BasicRRTStrategy::m_numDirections {1}

protected

Expansion directions per iteration.

◆ m_samplerLabel

std::string BasicRRTStrategy::m_samplerLabel

protected

The sampler label.

◆ m_trees

std::vector<VertexSet> BasicRRTStrategy::m_trees

protected

The current tree set.

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

/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/MPStrategies/BasicRRTStrategy.h
/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/MPStrategies/BasicRRTStrategy.cpp

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ VertexSet

◆ VID

Constructor & Destructor Documentation

◆ BasicRRTStrategy() [1/2]

◆ BasicRRTStrategy() [2/2]

◆ ~BasicRRTStrategy()

Member Function Documentation

◆ AddEdge()

◆ AddNode()

◆ ConnectNeighbors()

◆ ConnectTrees()

◆ ExpandTree() [1/2]

◆ ExpandTree() [2/2]

◆ Extend() [1/2]

◆ Extend() [2/2]

◆ FindNearestNeighbor()

◆ Initialize()

◆ Iterate()

◆ Print()

◆ SelectDispersedTarget()

◆ SelectNeighbor()

◆ SelectTarget()

◆ TryGoalExtension() [1/2]

◆ TryGoalExtension() [2/2]

Field Documentation

◆ m_disperseTrials

◆ m_exLabel

◆ m_fallbackNfLabel

◆ m_goalDmLabel

◆ m_goalThreshold

◆ m_growGoals

◆ m_growthFocus

◆ m_ncLabel

◆ m_nfLabel

◆ m_numDirections

◆ m_samplerLabel

◆ m_trees