Public Member Functions
Construction
	ClearanceUtility (string _vcLabel="", string _dmLabel="", bool _exactClearance=false, bool _exactPenetration=false, size_t _clearanceRays=10, size_t _penetrationRays=10, double _approxStepSize=MAX_DBL, double _approxResolution=MAX_DBL, bool _useBBX=true, bool _positional=true, bool _debug=false)

	ClearanceUtility (XMLNode &_node)

virtual	~ClearanceUtility ()=default

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

virtual void	Initialize () override

Accessors
const string &	GetDistanceMetricLabel () const
	Get the label of the distance metric.

const string &	GetValidityCheckerLabel () const
	Get the label of the validity checker.

void	SetValidityCheckerLabel (const string &_s)

bool	GetExactClearance () const
	Get whether exact clearance is used or not.

double	GetPositionResolution () const
	Get the position resolution.

double	GetOrientationResolution () const
	Get the orientation resolution.

bool	IsInitialized () const
	Get whether the object is initialized or not.

Clearance Functions
bool	CollisionInfo (CfgType &_cfg, CfgType &_clrCfg, const Boundary *const _b, CDInfo &_cdInfo, const bool &_useOppValidityWitness=true)
	Calculate clearance information for the medial axis computation.

bool	ExactCollisionInfo (CfgType &_cfg, CfgType &_clrCfg, const Boundary *const _b, CDInfo &_cdInfo, const bool _useOppValidityWitness=true)

bool	ApproxCollisionInfo (CfgType &_cfg, CfgType &_clrCfg, const Boundary *const _b, CDInfo &_cdInfo, const bool &_useOppValidityWitness=true)

bool	FindApproximateWitness (const std::size_t &_numRays, std::vector< Ray< CfgType > > &_rays, const CfgType &_sampledCfg, const bool &_initValidity, const Boundary *const _b, std::pair< size_t, CfgType > &_candidate, const bool &_useOppValidityWitness=true)

void	MakeRays (const CfgType &_sampledCfg, const std::size_t &_numRays, std::vector< Ray< CfgType > > &_rays)
	Generates the rays for the witness finding.

bool	ValidateCandidate (const std::pair< size_t, CfgType > &_cand, const std::vector< Ray< CfgType > > &_rays, const Boundary *const _b, const bool &_useOppValidityWitness=true)
	A final sanity check to be performed on the witness candidate.

ClearanceStats	RoadmapClearance ()

ClearanceStats	PathClearance (vector< VID > &_path)

ClearanceStats	PathClearance (vector< Cfg > &_path)

vector< double >	EdgeClearance (const CfgType &_c1, const CfgType &_c2, const WeightType &_weight)

I/O
virtual void	Print (std::ostream &_os) const

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

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
bool	AdjustWitnessToEnsureValidity (const CfgType &_cfg, const bool _initValidity, CfgType &_witnessCfg, const Boundary *const _b, const bool _useOppValidityWitness=true)

const bool	GetNearestVertexWitness (CfgType &_cfg, CDInfo &_cdInfo, const Boundary *const _b)

void	SetName (const std::string &_s)

const std::string &	GetBaseFilename () const

Protected Attributes
bool	m_debug
	Print debug info?

Internal State
string	m_vcLabel {"cd4"}
	Validity checker method label.

string	m_dmLabel {"euclidean"}
	Distance metric method label.

bool	m_useBBX {true}
	Use bounding box as obstacle?

bool	m_positionalDofsOnly {true}
	Use only positional dofs?

bool	m_exactClearance {false}
	Use exact clearance calculations?

bool	m_exactPenetration {false}
	Use exact penetration calculations?

size_t	m_clearanceRays {10}
	Number of rays for approximate clearance.

size_t	m_penetrationRays {10}
	Number of rays for approximate penetration.

double	m_rayTickResolution
	The resolution for rayTicks.

double	m_orientationResolution
	The orientation resolution.

double	m_maxRayMagnitude
	How far out a ray will be attempted.

size_t	m_maxRayIterations
	The maximum number of ray iterations.

double	m_orientationResFactor {0.}
	The orientation resolution factor.

double	m_positionalResFactor {0.}
	The position resoluiton factor.

double	m_maSearchResolutionFactor {1}
	The search resolution factor.

bool	m_initialized {false}

Detailed Description

template<class MPTraits>
class ClearanceUtility< MPTraits >

Computes clearance and penetration of configurations with respect to obstacles.

Constructor & Destructor Documentation

◆ ClearanceUtility() [1/2]

template<class MPTraits >

ClearanceUtility< MPTraits >::ClearanceUtility	(	string	_vcLabel = `""`,
		string	_dmLabel = `""`,
		bool	_exactClearance = `false`,
		bool	_exactPenetration = `false`,
		size_t	_clearanceRays = `10`,
		size_t	_penetrationRays = `10`,
		double	_approxStepSize = `MAX_DBL`,
		double	_approxResolution = `MAX_DBL`,
		bool	_useBBX = `true`,
		bool	_positional = `true`,
		bool	_debug = `false`
	)

Construct a ClearanceUtility object

Parameters

_vcLabel	The validity checker to use.
_dmLabel	The distance metric to use.
_exactClearance	Use exact clearance or not.
_exactPenetration	Use exact penetration or not.
_clearanceRays	The number of clearance rays to use.
_penetrationRays	The number of penetration rays to use.
_approxStepSize	The approximate step size to use.
_approxResolution	The approximate resolution to use.
_useBBX	Use BBX or not.
_positional	Use only positional DOFs.
_debug	Set to debug mode or not.

◆ ClearanceUtility() [2/2]

template<class MPTraits >

ClearanceUtility< MPTraits >::ClearanceUtility ( XMLNode & _node )

Construct a ClearanceUtility object from an XML node

Parameters

_node The XML node to use

Member Function Documentation

◆ Print() [1/2]

template<class MPTraits >

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

overridevirtual

Print the internal state of this object

Parameters

_os	The std::ostream to print to.

Reimplemented in MedialAxisUtility< MPTraits >.

◆ Initialize()

template<class MPTraits >

void ClearanceUtility< MPTraits >::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< MPTraits >.

◆ SetValidityCheckerLabel()

template<class MPTraits >

void ClearanceUtility< MPTraits >::SetValidityCheckerLabel ( const string & _s )

Set the label of the validity checker

Parameters

_s	The label of the validity checker to use.

◆ ExactCollisionInfo()

template<class MPTraits >

bool ClearanceUtility< MPTraits >::ExactCollisionInfo	(	CfgType &	_cfg,
		CfgType &	_clrCfg,
		const Boundary *const	_b,
		CDInfo &	_cdInfo,
		const bool	_useOppValidityWitness = `true`
	)

Calculate clearance information exactly by taking the validity checker results against obstacles to the bounding box.

◆ ApproxCollisionInfo()

template<class MPTraits >

bool ClearanceUtility< MPTraits >::ApproxCollisionInfo	(	CfgType &	_cfg,
		CfgType &	_clrCfg,
		const Boundary *const	_b,
		CDInfo &	_cdInfo,
		const bool &	_useOppValidityWitness = `true`
	)

Calculate the approximate clearance using a series of rays. The specified number of rays are pushed outward until they change in validity. The shortest ray is then considered the best candidate.

◆ FindApproximateWitness()

template<class MPTraits >

bool ClearanceUtility< MPTraits >::FindApproximateWitness	(	const std::size_t &	_numRays,
		std::vector< Ray< CfgType > > &	_rays,
		const CfgType &	_sampledCfg,
		const bool &	_initValidity,
		const Boundary *const	_b,
		std::pair< size_t, CfgType > &	_candidate,
		const bool &	_useOppValidityWitness = `true`
	)

Helpers for ApproxCollisionInfo (should maybe be protected): Find the witness of an initial sample of any validity. A witness here is defined as the nearest point of obstacle boundary. @TODO Make the user have the option to have same or opposite validity for witnesses.

◆ RoadmapClearance()

template<class MPTraits >

ClearanceStats ClearanceUtility< MPTraits >::RoadmapClearance

Calculate roadmap clearance statistics including averages, mins, and maxes for clearance across roadmaps, paths, and edges.

◆ PathClearance() [1/2]

template<class MPTraits >

ClearanceStats ClearanceUtility< MPTraits >::PathClearance ( vector< VID > & _path )

Calculate path clearance statistics including averages, mins, and maxes for clearance across the path

Parameters

_path The path to use.

◆ PathClearance() [2/2]

template<class MPTraits >

ClearanceStats ClearanceUtility< MPTraits >::PathClearance ( vector< Cfg > & _path )

Calculate path clearance statistics including averages, mins, and maxes for clearance across the path

Parameters

_path The path to use.

◆ EdgeClearance()

template<class MPTraits >

vector< double > ClearanceUtility< MPTraits >::EdgeClearance	(	const CfgType &	_c1,
		const CfgType &	_c2,
		const WeightType &	_weight
	)

Calculate path clearance statistics including averages, mins, and maxes for clearance across the edge

Parameters

_c1	The source vertex.
_c2	The target vertex.
_weight	The weight of the edge.

◆ AdjustWitnessToEnsureValidity()

template<class MPTraits >

bool ClearanceUtility< MPTraits >::AdjustWitnessToEnsureValidity	(	const CfgType &	_cfg,
		const bool	_initValidity,
		CfgType &	_witnessCfg,
		const Boundary *const	_b,
		const bool	_useOppValidityWitness = `true`
	)

protected

Adjusts a witness from a nearby witness point to a point of guaranteed validity, based on _initValidity and _useOppValidityWitness.

◆ GetNearestVertexWitness()

template<class MPTraits >

const bool ClearanceUtility< MPTraits >::GetNearestVertexWitness	(	CfgType &	_cfg,
		CDInfo &	_cdInfo,
		const Boundary *const	_b
	)

protected

GetNearestVertexWitness will do this for us, including boundary witness information, if BBX is active as an obstacle.

◆ Print() [2/2]

template<typename MPTraits >

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

virtualinherited

◆ 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

Member Data Documentation

◆ m_initialized

template<class MPTraits >

bool ClearanceUtility< MPTraits >::m_initialized {false}

protected

A boolean to determine whether initialize has been called on the utility or not yet. This is important, as there are MPLibrary things we need that are not available at construction time:

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

MedialAxisUtilities.h

Public Member Functions

Public Types

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ClearanceUtility() [1/2]

◆ ClearanceUtility() [2/2]

Member Function Documentation

◆ Print() [1/2]

◆ Initialize()

◆ SetValidityCheckerLabel()

◆ ExactCollisionInfo()

◆ ApproxCollisionInfo()

◆ FindApproximateWitness()

◆ RoadmapClearance()

◆ PathClearance() [1/2]

◆ PathClearance() [2/2]

◆ EdgeClearance()

◆ AdjustWitnessToEnsureValidity()

◆ GetNearestVertexWitness()

◆ Print() [2/2]

◆ SetName()

◆ GetBaseFilename()

Member Data Documentation

◆ m_initialized