#include <DynamicRegionRRT.h>

Inheritance diagram for DynamicRegionRRT:

[legend]

Collaboration diagram for DynamicRegionRRT:

[legend]

Data Structures
struct	SamplingRegion
	Representation of a sampling region. More...

Public Types
Motion Planning Types
typedef MPBaseObject::WeightType	WeightType

typedef MPBaseObject::RoadmapType	RoadmapType

typedef RoadmapType::VID	VID

WorkspaceSkeleton Types
using	SkeletonEdgeDescriptor = WorkspaceSkeleton::ED

using	SkeletonEdgeIterator = WorkspaceSkeleton::adj_edge_iterator

using	SkeletonVertexDescriptor = WorkspaceSkeleton::vertex_descriptor

using	SkeletonVertexIterator = WorkspaceSkeleton::vertex_iterator

Public Types inherited from BasicRRTStrategy
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

	DynamicRegionRRT ()

	DynamicRegionRRT (XMLNode &_node)

virtual	~DynamicRegionRRT ()=default

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

Public Member Functions inherited from BasicRRTStrategy
	BasicRRTStrategy ()

	BasicRRTStrategy (XMLNode &_node)

virtual	~BasicRRTStrategy ()

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
MPStrategyMethod Overrides
virtual void	Initialize () override

BasicRRTStrategy Overrides
virtual Cfg	SelectTarget () override
	Get a random configuration to grow towards. More...

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

Helpers
Cfg	Sample (SamplingRegion *_region)

Cfg	Sample (const Boundary const _boundary, const std::string _samplerLabel)

const Vector3d	GetVelocityBias (SamplingRegion *_region)

bool	IsTouching (const Cfg &_cfg, SamplingRegion &_region)

CSpaceBoundingSphere	MakeBoundary (const Vector3d &_v)

Skeleton and Workspace
void	BuildSkeleton ()
	Build topological skeleton. More...

void	DirectSkeleton ()
	Construct the pruned and directed query skeleton. More...

const size_t	SelectSamplingRegion ()

std::vector< double >	ComputeProbabilities ()

void	BiasVelocity (Cfg &_cfg, SamplingRegion *_region)

void	CheckRegionProximity (const Point3d &_p)

std::vector< SamplingRegion * >	CreateRegions (const WorkspaceSkeleton::vertex_iterator _iter)

void	AdvanceRegions (const Cfg &_cfg)

bool	AdvanceRegionToCompletion (const Cfg &_cfg, SamplingRegion &_region)

Protected Member Functions inherited from BasicRRTStrategy
virtual void	Iterate () override
	Execute one iteration of the strategy. More...

Cfg	SelectDispersedTarget (const VID _v)

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

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

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

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
Internal State
WorkspaceSkeleton	m_originalSkeleton
	The original workspace skeleton. More...

WorkspaceSkeleton	m_skeleton
	The directed/pruned workspace skeleton. More...

std::string	m_skeletonType {"reeb"}
	Type of skeleton to build. More...

std::string	m_decompositionLabel
	The workspace decomposition label. More...

std::string	m_scuLabel
	The skeleton clearance utility label. More...

bool	m_velocityBiasing {false}
	Use velocity biasing? More...

double	m_velocityAlignment {.1}
	Strength of velocity biasing. More...

bool	m_initialized {false}
	Have auxiliary structures been initialized? More...

std::pair< Point3d, Point3d >	m_queryPair
	Pair of points we use to direct the skeleton. More...

std::vector< SamplingRegion >	m_regions
	The set of active dynamic sampling regions and associated metadata. More...

std::unordered_map< WorkspaceSkeleton::VD, bool >	m_visited
	Keep track of which skeleton vertices we've visited. More...

double	m_regionFactor {2}

double	m_regionRadius {0}
	The region radius. More...

double	m_explore {.5}

double	m_penetrationFactor {1}

Protected Attributes inherited from BasicRRTStrategy
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...

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...

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

Dynamic Region-biased RRT algorithm.

An RRT guided by a workspace skeleton.

Reference: Jory Denny, Read Sandstrom, Andrew Bregger, and Nancy M. Amato. "Dynamic Region-biased Rapidly-exploring Random Trees." WAFR 2016.

Member Typedef Documentation

◆ RoadmapType

typedef MPBaseObject::RoadmapType DynamicRegionRRT::RoadmapType

◆ SkeletonEdgeDescriptor

using DynamicRegionRRT::SkeletonEdgeDescriptor = WorkspaceSkeleton::ED

◆ SkeletonEdgeIterator

using DynamicRegionRRT::SkeletonEdgeIterator = WorkspaceSkeleton::adj_edge_iterator

◆ SkeletonVertexDescriptor

using DynamicRegionRRT::SkeletonVertexDescriptor = WorkspaceSkeleton::vertex_descriptor

◆ SkeletonVertexIterator

using DynamicRegionRRT::SkeletonVertexIterator = WorkspaceSkeleton::vertex_iterator

◆ VID

typedef RoadmapType::VID DynamicRegionRRT::VID

◆ WeightType

typedef MPBaseObject::WeightType DynamicRegionRRT::WeightType

Constructor & Destructor Documentation

◆ DynamicRegionRRT() [1/2]

DynamicRegionRRT::DynamicRegionRRT ( )

@ Construction

◆ DynamicRegionRRT() [2/2]

DynamicRegionRRT::DynamicRegionRRT ( XMLNode & _node )

◆ ~DynamicRegionRRT()

virtual DynamicRegionRRT::~DynamicRegionRRT ( )

virtualdefault

Member Function Documentation

◆ AddNode()

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

overrideprotectedvirtual

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 from BasicRRTStrategy.

◆ AdvanceRegions()

void DynamicRegionRRT::AdvanceRegions ( const Cfg & _cfg )

protected

Advance all sampling regions until they are no longer touching the newly added configuration.

Parameters

_cfg	The newly added configuration, q_new.

◆ AdvanceRegionToCompletion()

bool DynamicRegionRRT::AdvanceRegionToCompletion	(	const Cfg &	_cfg,
		SamplingRegion &	_region
	)

protected

Advance a region until it is either not longer touching a configuration or until it reaches the end of its respective skeleton edge.

Parameters

_cfg	A configuration possibly touching the region.
_region	The region to advance along its skeleton edge.

◆ BiasVelocity()

void DynamicRegionRRT::BiasVelocity	(	Cfg &	_cfg,
		SamplingRegion *	_region
	)

protected

Bias the velocity of a sample along a direction perscribed by the region.

Parameters

_cfg	The sample to bias.
_region	The region from which _cfg was sampled.

◆ BuildSkeleton()

void DynamicRegionRRT::BuildSkeleton ( )

protected

Build topological skeleton.

◆ CheckRegionProximity()

void DynamicRegionRRT::CheckRegionProximity ( const Point3d & _p )

protected

Check if q_new is close enough to an unvisited skeleton vertex to create new regions on the outgoing edges of that vertex. If so, create those new regions.

Parameters

_p	The new configuration added to the roadmap.

◆ ComputeProbabilities()

std::vector< double > DynamicRegionRRT::ComputeProbabilities ( )

protected

Compute probabilities for selecting each sampling region.

Returns: Probabilities based on extension success

◆ CreateRegions()

std::vector< typename DynamicRegionRRT::SamplingRegion * > DynamicRegionRRT::CreateRegions ( const WorkspaceSkeleton::vertex_iterator _iter )

protected

Create new regions on the outgoing edges of the skeleton vertex.

Parameters

_iter The skeleton vertex iterator.

Returns: The newly created sampling regions.

◆ DirectSkeleton()

void DynamicRegionRRT::DirectSkeleton ( )

protected

Construct the pruned and directed query skeleton.

◆ GetVelocityBias()

const Vector3d DynamicRegionRRT::GetVelocityBias ( SamplingRegion * _region )

protected

Calculate the velocity bias along a region's skeleton edge.

Parameters

_region The region whose skeleton edge to bias the velocity along.

Returns: The velocity bias.

◆ Initialize()

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

◆ IsTouching()

bool DynamicRegionRRT::IsTouching	(	const Cfg &	_cfg,
		SamplingRegion &	_region
	)

protected

Determine if a region is touching a configuration.

Parameters

_cfg	The configuration.
_region	The sampling region.

◆ MakeBoundary()

CSpaceBoundingSphere DynamicRegionRRT::MakeBoundary ( const Vector3d & _v )

protected

Calculate the boundary around a sampling region.

Parameters

_v	The center of the sampling region.

Returns: The boundary with center _v and radius m_regionRadius.

◆ Print()

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

overridevirtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from BasicRRTStrategy.

◆ Sample() [1/2]

Cfg DynamicRegionRRT::Sample	(	const Boundary *const	_boundary,
		const std::string *	_samplerLabel
	)

protected

Sample a configuration from within a given boundary using the sampler given in _samplerLabel.

Parameters

_region The region to sample from.

Returns: A configuration with the boundary.

◆ Sample() [2/2]

Cfg DynamicRegionRRT::Sample ( SamplingRegion * _region )

protected

Sample a configuration from within a sampling region using the sampler given in m_samplerLabel.

Parameters

_region The region to sample from.

Returns: A configuration with the sampling region.

◆ SelectSamplingRegion()

const size_t DynamicRegionRRT::SelectSamplingRegion ( )

protected

Select a region based on weighted success probabilities

Returns: The sampling region to be expanded

◆ SelectTarget()

Cfg DynamicRegionRRT::SelectTarget ( )

overrideprotectedvirtual

Get a random configuration to grow towards.

Reimplemented from BasicRRTStrategy.

Field Documentation

◆ m_decompositionLabel

std::string DynamicRegionRRT::m_decompositionLabel

protected

The workspace decomposition label.

◆ m_explore

double DynamicRegionRRT::m_explore {.5}

protected

Weight of explore vs. exploit in region selection probabilities. Exploring is a uniform chance to select each region, while exploit favors successful regions.

◆ m_initialized

bool DynamicRegionRRT::m_initialized {false}

protected

Have auxiliary structures been initialized?

◆ m_originalSkeleton

WorkspaceSkeleton DynamicRegionRRT::m_originalSkeleton

protected

The original workspace skeleton.

◆ m_penetrationFactor

double DynamicRegionRRT::m_penetrationFactor {1}

protected

A configuration is considered to be touching a region when this fraction of its bounding sphere penetrates into the region.

◆ m_queryPair

std::pair<Point3d, Point3d> DynamicRegionRRT::m_queryPair

protected

Pair of points we use to direct the skeleton.

◆ m_regionFactor

double DynamicRegionRRT::m_regionFactor {2}

protected

The dynamic sampling regions will have radius equal to this times the robot's bounding sphere radius.

◆ m_regionRadius

double DynamicRegionRRT::m_regionRadius {0}

protected

The region radius.

◆ m_regions

std::vector<SamplingRegion> DynamicRegionRRT::m_regions

protected

The set of active dynamic sampling regions and associated metadata.

◆ m_scuLabel

std::string DynamicRegionRRT::m_scuLabel

protected

The skeleton clearance utility label.

◆ m_skeleton

WorkspaceSkeleton DynamicRegionRRT::m_skeleton

protected

The directed/pruned workspace skeleton.

◆ m_skeletonType

std::string DynamicRegionRRT::m_skeletonType {"reeb"}

protected

Type of skeleton to build.

◆ m_velocityAlignment

double DynamicRegionRRT::m_velocityAlignment {.1}

protected

Strength of velocity biasing.

◆ m_velocityBiasing

bool DynamicRegionRRT::m_velocityBiasing {false}

protected

Use velocity biasing?

◆ m_visited

std::unordered_map<WorkspaceSkeleton::VD, bool> DynamicRegionRRT::m_visited

protected

Keep track of which skeleton vertices we've visited.

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

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

Data Structures

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ RoadmapType

◆ SkeletonEdgeDescriptor

◆ SkeletonEdgeIterator

◆ SkeletonVertexDescriptor

◆ SkeletonVertexIterator

◆ VID

◆ WeightType

Constructor & Destructor Documentation

◆ DynamicRegionRRT() [1/2]

◆ DynamicRegionRRT() [2/2]

◆ ~DynamicRegionRRT()

Member Function Documentation

◆ AddNode()

◆ AdvanceRegions()

◆ AdvanceRegionToCompletion()

◆ BiasVelocity()

◆ BuildSkeleton()

◆ CheckRegionProximity()

◆ ComputeProbabilities()

◆ CreateRegions()

◆ DirectSkeleton()

◆ GetVelocityBias()

◆ Initialize()

◆ IsTouching()

◆ MakeBoundary()

◆ Print()

◆ Sample() [1/2]

◆ Sample() [2/2]

◆ SelectSamplingRegion()

◆ SelectTarget()

Field Documentation

◆ m_decompositionLabel

◆ m_explore

◆ m_initialized

◆ m_originalSkeleton

◆ m_penetrationFactor

◆ m_queryPair

◆ m_regionFactor

◆ m_regionRadius

◆ m_regions

◆ m_scuLabel

◆ m_skeleton

◆ m_skeletonType

◆ m_velocityAlignment

◆ m_velocityBiasing

◆ m_visited