DynamicRegionsPRM Class Reference

#include <DynamicRegionsPRM.h>

Inheritance diagram for DynamicRegionsPRM:

Collaboration diagram for DynamicRegionsPRM:

Data Structures
struct	EdgeCompare
struct	EdgeIDCompare
struct	EdgeOutput
	Output for connection attempt. More...
struct	ExpansionRegion
	Representation of an expansion region. More...
struct	LocalComponentDescriptor
	A descriptor for a local connected component. More...
struct	SamplerSetting
	Settings for a specific sampler. More...

Public Types
Motion Planning Types
typedef MPBaseObject::WeightType	WeightType
typedef MPBaseObject::RoadmapType	RoadmapType
typedef RoadmapType::VID	VID
typedef RoadmapType::VertexSet	VertexSet
WorkspaceSkeleton Types
using	SkeletonEdgeDescriptor = WorkspaceSkeleton::ED
using	SkeletonEdgeIterator = WorkspaceSkeleton::adj_edge_iterator
using	SkeletonVertexDescriptor = WorkspaceSkeleton::vertex_descriptor
using	SkeletonVertexIterator = WorkspaceSkeleton::vertex_iterator
Local Types
typedef std::map< Robot *, std::map< SkeletonEdgeDescriptor, std::map< VID, VertexSet >, EdgeCompare > >	LocalConnectivityMap
typedef std::map< Robot *, std::map< SkeletonEdgeDescriptor, std::map< VID, VertexSet >, EdgeIDCompare > >	BridgeMap
	Bridges are non-directional and tied to edge IDs. More...
typedef std::map< Robot *, std::map< SkeletonEdgeDescriptor, std::map< VID, ExpansionRegion >, EdgeCompare > >	RegionMap
	Map for regions. More...
typedef std::map< Robot *, std::set< size_t > >	LowClearanceMap
	Map for low clearance. More...
typedef std::map< Robot *, std::set< SkeletonEdgeDescriptor, EdgeCompare > >	UnconnectedEdgeMap
	Map for regions with unconnected local components. More...
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
	DynamicRegionsPRM ()
	DynamicRegionsPRM (XMLNode &_node)
virtual	~DynamicRegionsPRM ()=default
MPBaseObject Overrides
virtual void	Print (std::ostream &_os) const override
MPStrategyMethod Overrides
virtual void	Initialize () override
virtual void	Iterate () override
	Execute one iteration of the strategy. More...
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
VertexSet	ExpandComponent (ExpansionRegion *const _r)
void	ConnectEdgeSegment (const SkeletonEdgeDescriptor _ed)
void	InitializeLUnconnectedEdgeMap ()
Helpers
void	AddQuery ()
	Add start and goals to the roadmap. More...
bool	CheckReachedRegion (const VID _query, const VertexSet &_component)
	Hax. More...
void	GrowRRT (const VID _q)
Local Connected Components
void	MakeLocalComponent (const LocalComponentDescriptor &_d, const VertexSet &_vids)
VertexSet &	GetLocalComponent (const LocalComponentDescriptor &_d) noexcept
ExpansionRegion *	GetExpansionRegion (const LocalComponentDescriptor &_d) noexcept
VertexSet &	GetBridge (const LocalComponentDescriptor &_d) noexcept
LocalComponentDescriptor	FindLocalComponent (const VID _vid) const noexcept
LocalComponentDescriptor	PromoteLocalComponent (LocalComponentDescriptor _d)
LocalComponentDescriptor	MergeLocalComponents (const LocalComponentDescriptor &_d1, const LocalComponentDescriptor &_d2)
void	UpdateEdgeConnectivity (const SkeletonEdgeDescriptor &_ed)
	Update the local connectivity for an edge. More...
bool	IsEdgeConnected (const SkeletonEdgeDescriptor &_ed)
	Check if an edge is considered locally connected. More...
Region Functions
ExpansionRegion *	SelectExpansionRegion ()
std::pair< std::vector< double >, std::vector< ExpansionRegion * > >	ComputeProbabilities ()
bool	AdvanceRegion (ExpansionRegion *const _r, const VertexSet &_newVIDs)
bool	AreSamplesCovered (const ExpansionRegion *const _region, const VertexSet &_samples)
General Planning
std::vector< Cfg >	Sample (const Boundary *_b=nullptr)
std::vector< Neighbor >	FindNearestNeighbors (const Cfg &_cfg, const VertexSet *const _candidates)
	Return K nearest neighors from a set of candidates. More...
bool	AttemptConnection (const Cfg &_c1, const Cfg &_c2, LPOutput &_lpOuptut)
std::vector< EdgeOutput >	ConnectToComponent (const Cfg &_cfg, const LocalComponentDescriptor &_d)
VID	Extend (const VID _nearVID, const Cfg &_target, LPOutput &_lp)
	Extend a tree node towards a direction. More...
Workspace and Clearance
CSpaceBoundingSphere	MakeBoundary (const Vector3d &_v)
double	GetRegionRadius (const Vector3d &_v)
double	GetClearance (const Vector3d &_v)
void	BuildSkeleton ()
	Build topological skeleton. More...
void	InitializeLowClearanceMap ()
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
std::vector< SamplerSetting >	m_samplers
	Samplers to generate nodes. More...
std::string	m_nfLabel
	The neighborhood finder label. More...
std::string	m_lpLabel
	The neighborhood finder label. More...
std::string	m_exLabel
	The extender label. More...
std::string	m_decompositionLabel
	The workspace decomposition label. More...
std::string	m_skeletonType {"reeb"}
	Type of skeleton to build. More...
std::string	m_skeletonFilename
	The output file for the skeleton graph. More...
std::string	m_skeletonIO
	Option to read or write the skeleton. More...
double	m_explore {.5}
double	m_minRegionRadius {-1}
bool	m_aggressiveBridging {true}
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...

Static Protected Attributes
Shared State
static bool	m_initialized = false
	Is the shared state initialized? More...
static WorkspaceSkeleton	m_skeleton
	The workspace skeleton. More...
static LocalConnectivityMap	m_localComponents
	Local components in progress. More...
static BridgeMap	m_bridges
	Completed local components. More...
static RegionMap	m_regions
	Expansion regions for local components. More...
static LowClearanceMap	m_lowClearanceMap
	Track low-clearance edges. More...
static UnconnectedEdgeMap	m_unconnectedEdges
	Track unconnected edges. More...

Detailed Description

Dynamic Regions PRM algorithm.

A PRM guided by a workspace skeleton.

Reference: Read Sandstrom. "Approximating Configuration Space Topology with Workspace Models". PhD Thesis, Spring 2020. -and- Read Sandstrom, Diane Uwacu, Jory Denny, and Nancy M. Amato. "Topology-Guided Roadmap Construction with Dynamic Region Sampling". Under review for RA-L @ IROS 20.

Member Typedef Documentation

BridgeMap

typedef std::map<Robot*, std::map<SkeletonEdgeDescriptor, std::map<VID, VertexSet>, EdgeIDCompare> > DynamicRegionsPRM::BridgeMap

Bridges are non-directional and tied to edge IDs.

LocalConnectivityMap

typedef std::map<Robot*, std::map<SkeletonEdgeDescriptor, std::map<VID, VertexSet>, EdgeCompare> > DynamicRegionsPRM::LocalConnectivityMap

Map for local connected components. Maps skeleton edge to representative to vids.

LowClearanceMap

typedef std::map<Robot*, std::set<size_t> > DynamicRegionsPRM::LowClearanceMap

Map for low clearance.

RegionMap

typedef std::map<Robot*, std::map<SkeletonEdgeDescriptor, std::map<VID, ExpansionRegion>, EdgeCompare> > DynamicRegionsPRM::RegionMap

Map for regions.

RoadmapType

typedef MPBaseObject::RoadmapType DynamicRegionsPRM::RoadmapType

SkeletonEdgeDescriptor

using DynamicRegionsPRM::SkeletonEdgeDescriptor = WorkspaceSkeleton::ED

SkeletonEdgeIterator

using DynamicRegionsPRM::SkeletonEdgeIterator = WorkspaceSkeleton::adj_edge_iterator

SkeletonVertexDescriptor

using DynamicRegionsPRM::SkeletonVertexDescriptor = WorkspaceSkeleton::vertex_descriptor

SkeletonVertexIterator

using DynamicRegionsPRM::SkeletonVertexIterator = WorkspaceSkeleton::vertex_iterator

UnconnectedEdgeMap

typedef std::map<Robot*, std::set<SkeletonEdgeDescriptor, EdgeCompare> > DynamicRegionsPRM::UnconnectedEdgeMap

Map for regions with unconnected local components.

VertexSet

typedef RoadmapType::VertexSet DynamicRegionsPRM::VertexSet

VID

typedef RoadmapType::VID DynamicRegionsPRM::VID

WeightType

typedef MPBaseObject::WeightType DynamicRegionsPRM::WeightType

Constructor & Destructor Documentation

DynamicRegionsPRM() [1/2]

DynamicRegionsPRM::DynamicRegionsPRM

(

)

DynamicRegionsPRM() [2/2]

DynamicRegionsPRM::DynamicRegionsPRM

(

XMLNode &

_node

)

~DynamicRegionsPRM()

virtual DynamicRegionsPRM::~DynamicRegionsPRM ( )

virtualdefault

Member Function Documentation

AddQuery()

void DynamicRegionsPRM::AddQuery ( )

protected

Add start and goals to the roadmap.

AdvanceRegion()

bool DynamicRegionsPRM::AdvanceRegion ( ExpansionRegion *const  _r, const VertexSet &  _newVIDs  )

protected

Advance a region along an edge.

Parameters

_r	The region to advance.
_newVIDs	The recently added VIDs from sampling in this region.

Returns

True if the region is successfully advanced, false if the region reaches end of the edge or an low clearance area.

AreSamplesCovered()

bool DynamicRegionsPRM::AreSamplesCovered ( const ExpansionRegion *const  _region, const VertexSet &  _samples  )

protected

Check if region still covers a subset of the given samples set

Parameters

_region	The region to check.
_samples	The newly generated samples to check.

Returns

True if any of _samples is contained by _region.

AttemptConnection()

bool DynamicRegionsPRM::AttemptConnection ( const Cfg &  _c1, const Cfg &  _c2, LPOutput &  _lpOuptut  )

protected

Attempt connections between a configuration and its neighbors

Returns

the set of neighbors that successfully connect to _c

BuildSkeleton()

void DynamicRegionsPRM::BuildSkeleton ( )

protected

Build topological skeleton.

CheckReachedRegion()

bool DynamicRegionsPRM::CheckReachedRegion ( const VID  _query, const VertexSet &  _component  )

protected

Hax.

ComputeProbabilities()

std::pair< std::vector< double >, std::vector< typename DynamicRegionsPRM::ExpansionRegion * > > DynamicRegionsPRM::ComputeProbabilities ( )

protected

Compute probabilities for selecting each expansion region.

Returns

probabiliities based on expansion success

ConnectEdgeSegment()

void DynamicRegionsPRM::ConnectEdgeSegment ( const SkeletonEdgeDescriptor  _ed )

protected

Connect local components within an edge segment.

Parameters

_ed	The edge segment to connect.

ConnectToComponent()

std::vector< typename DynamicRegionsPRM::EdgeOutput > DynamicRegionsPRM::ConnectToComponent ( const Cfg &  _cfg, const LocalComponentDescriptor &  _d  )

protected

Try to connect a configuration to a local component.

Parameters

_cfg	The joining sample.
_d	The component descriptor.

Returns

The generated edges.

ExpandComponent()

DynamicRegionsPRM::RoadmapType::VertexSet DynamicRegionsPRM::ExpandComponent ( ExpansionRegion *const  _r )

protected

Expand a component in region r

Returns

the list of new cfgs added to the component

Extend()

DynamicRegionsPRM::VID DynamicRegionsPRM::Extend ( const VID  _nearVID, const Cfg &  _target, LPOutput &  _lp  )

protected

Extend a tree node towards a direction.

FindLocalComponent()

DynamicRegionsPRM::LocalComponentDescriptor DynamicRegionsPRM::FindLocalComponent ( const VID  _vid ) const

protectednoexcept

Get the local component descriptor for a vertex.

Warning

This is a linear scan over all local CCs, use very sparingly.

FindNearestNeighbors()

std::vector< Neighbor > DynamicRegionsPRM::FindNearestNeighbors ( const Cfg &  _cfg, const VertexSet *const  _candidates  )

protected

Return K nearest neighors from a set of candidates.

GetBridge()

DynamicRegionsPRM::RoadmapType::VertexSet & DynamicRegionsPRM::GetBridge ( const LocalComponentDescriptor &  _d )

protectednoexcept

Get a bridge's vertex set.

Parameters

_d	The descriptor for the bridge.

Returns

The set of VIDs in this bridge.

GetClearance()

double DynamicRegionsPRM::GetClearance ( const Vector3d &  _v )

protected

Get clearance

Returns

robot clearance from closest obstacle

GetExpansionRegion()

DynamicRegionsPRM::ExpansionRegion * DynamicRegionsPRM::GetExpansionRegion ( const LocalComponentDescriptor &  _d )

protectednoexcept

Get the expansion region for a local component.

Parameters

_d	The descriptor for the local component.

Returns

The expansion region for this component.

GetLocalComponent()

DynamicRegionsPRM::RoadmapType::VertexSet & DynamicRegionsPRM::GetLocalComponent ( const LocalComponentDescriptor &  _d )

protectednoexcept

Get a local component's vertex set.

Parameters

_d	The descriptor for the local component.

Returns

The set of VIDs in this component.

GetRegionRadius()

double DynamicRegionsPRM::GetRegionRadius ( const Vector3d &  _v )

protected

Get region radius return radius = robot clearance - robot radius

GrowRRT()

void DynamicRegionsPRM::GrowRRT ( const VID  _q )

protected

Grow a tree from a vertex q Stop when q connects to the rest of the roadmap or is within the boundary of a skeleton vertex

Initialize()

void DynamicRegionsPRM::Initialize ( )

overridevirtual

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.

InitializeLowClearanceMap()

void DynamicRegionsPRM::InitializeLowClearanceMap ( )

protected

InitializeLUnconnectedEdgeMap()

void DynamicRegionsPRM::InitializeLUnconnectedEdgeMap ( )

protected

IsEdgeConnected()

bool DynamicRegionsPRM::IsEdgeConnected ( const SkeletonEdgeDescriptor &  _ed )

protected

Check if an edge is considered locally connected.

Iterate()

void DynamicRegionsPRM::Iterate ( )

overridevirtual

Execute one iteration of the strategy.

Reimplemented from MPStrategyMethod.

MakeBoundary()

CSpaceBoundingSphere DynamicRegionsPRM::MakeBoundary ( const Vector3d &  _v )

protected

Make a boundary

Returns

A boundary centered at _v

MakeLocalComponent()

void DynamicRegionsPRM::MakeLocalComponent ( const LocalComponentDescriptor &  _d, const VertexSet &  _vids  )

protected

Initialize tracking for a local connected component.

Parameters

_d	The descriptor for the local component.
_vids	All roadmap vertices initially in the component.

MergeLocalComponents()

DynamicRegionsPRM::LocalComponentDescriptor DynamicRegionsPRM::MergeLocalComponents ( const LocalComponentDescriptor &  _d1, const LocalComponentDescriptor &  _d2  )

protected

Merge two local connected components. If they are both incomplete and from the same side of the edge, the one whos region is further ahead will be preserved. If they are from different sides of the edge, a bridge will be formed.

Parameters

_d1	The descriptor for the first component.
_d2	The descriptor for the second component.

Returns

The descriptor of the surviving component.

Print()

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

overridevirtual

Print internal state of this object.

Parameters

_os	The std::ostream to print to.

Reimplemented from MPStrategyMethod.

PromoteLocalComponent()

DynamicRegionsPRM::LocalComponentDescriptor DynamicRegionsPRM::PromoteLocalComponent ( LocalComponentDescriptor  _d )

protected

Promote a local connected component to a bridge.

Parameters

_d	The descriptor for the local component.

Sample()

std::vector< Cfg > DynamicRegionsPRM::Sample ( const Boundary *  _b = nullptr )

protected

Sample and add configurations to the roadmap.

Returns

The generated VIDs for the successful samples.

SelectExpansionRegion()

DynamicRegionsPRM::ExpansionRegion * DynamicRegionsPRM::SelectExpansionRegion ( )

protected

Select a region based weighted success probabilities

Returns

expansion region to be expanded

UpdateEdgeConnectivity()

void DynamicRegionsPRM::UpdateEdgeConnectivity ( const SkeletonEdgeDescriptor &  _ed )

protected

Update the local connectivity for an edge.

Field Documentation

m_aggressiveBridging

bool DynamicRegionsPRM::m_aggressiveBridging {true}

protected

After extending a local component, aggressively try to bridge across to the other side from the newly added configurations.

m_bridges

DynamicRegionsPRM::BridgeMap DynamicRegionsPRM::m_bridges

staticprotected

Completed local components.

m_decompositionLabel

std::string DynamicRegionsPRM::m_decompositionLabel

protected

The workspace decomposition label.

m_exLabel

std::string DynamicRegionsPRM::m_exLabel

protected

The extender label.

m_explore

double DynamicRegionsPRM::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 DynamicRegionsPRM::m_initialized = false

staticprotected

Is the shared state initialized?

m_localComponents

DynamicRegionsPRM::LocalConnectivityMap DynamicRegionsPRM::m_localComponents

staticprotected

Local components in progress.

m_lowClearanceMap

DynamicRegionsPRM::LowClearanceMap DynamicRegionsPRM::m_lowClearanceMap

staticprotected

Track low-clearance edges.

m_lpLabel

std::string DynamicRegionsPRM::m_lpLabel

protected

The neighborhood finder label.

m_minRegionRadius

double DynamicRegionsPRM::m_minRegionRadius {-1}

protected

Optional minimum radius for regions. -1 indicates no limit, allowing low-clearance regions to be pruned.

m_nfLabel

std::string DynamicRegionsPRM::m_nfLabel

protected

The neighborhood finder label.

m_regions

DynamicRegionsPRM::RegionMap DynamicRegionsPRM::m_regions

staticprotected

Expansion regions for local components.

m_samplers

std::vector<SamplerSetting> DynamicRegionsPRM::m_samplers

protected

Samplers to generate nodes.

m_skeleton

WorkspaceSkeleton DynamicRegionsPRM::m_skeleton

staticprotected

The workspace skeleton.

m_skeletonFilename

std::string DynamicRegionsPRM::m_skeletonFilename

protected

The output file for the skeleton graph.

m_skeletonIO

std::string DynamicRegionsPRM::m_skeletonIO

protected

Option to read or write the skeleton.

m_skeletonType

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

protected

Type of skeleton to build.

m_unconnectedEdges

DynamicRegionsPRM::UnconnectedEdgeMap DynamicRegionsPRM::m_unconnectedEdges

staticprotected

Track unconnected edges.

---

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

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