#include <ClearanceUtilities.h>

Inheritance diagram for ClearanceUtility:

Collaboration diagram for ClearanceUtility:

[legend]

Public Types
Motion Planning Types
typedef MPBaseObject::WeightType	WeightType

typedef MPBaseObject::RoadmapType	RoadmapType

typedef RoadmapType::VID	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
	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 _positionalDofsOnly=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

const string &	GetValidityCheckerLabel () const

void	SetValidityCheckerLabel (const string &_s)

bool	GetExactClearance () const

double	GetPositionResolution () const

double	GetOrientationResolution () const

bool	IsInitialized () const

Clearance Functions
bool	CollisionInfo (Cfg &_cfg, Cfg &_clrCfg, const Boundary *const _b, CDInfo &_cdInfo, const bool &_useOppValidityWitness=true)

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

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

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

void	MakeRays (const Cfg &_sampledCfg, const std::size_t &_numRays, std::vector< Ray< Cfg > > &_rays)

bool	ValidateCandidate (const std::pair< size_t, Cfg > &_cand, const std::vector< Ray< Cfg > > &_rays, const Boundary *const _b, const bool &_useOppValidityWitness=true)

ClearanceStats	RoadmapClearance ()

ClearanceStats	PathClearance (vector< VID > &_path)

ClearanceStats	PathClearance (vector< Cfg > &_path)

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

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

void	SetRobot (Robot *_robot)

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

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

const std::string &	GetBaseFilename () const

Protected Attributes
int	abcd = 0

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

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

bool	m_useBBX {true}
	Use bounding box as obstacle? More...

bool	m_positionalDofsOnly {true}
	Use only positional dofs? More...

bool	m_exactClearance {false}
	Use exact clearance calculations? More...

bool	m_exactPenetration {false}
	Use exact penetration calculations? More...

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

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

double	m_rayTickResolution
	The resolution for rayTicks. More...

double	m_orientationResolution
	The orientation resolution. More...

double	m_maxRayMagnitude
	How far out a ray will be attempted. More...

size_t	m_maxRayIterations
	The maximum number of ray iterations. More...

double	m_orientationResFactor {1.}
	The orientation resolution factor. More...

double	m_positionalResFactor {1.}
	The position resoluiton factor. More...

double	m_maSearchResolutionFactor {1}
	The search resolution factor. More...

bool	m_initialized {false}

Robot *	m_robot

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

Detailed Description

Computes clearance and penetration of configurations with respect to obstacles.

Member Typedef Documentation

◆ RoadmapType

typedef MPBaseObject::RoadmapType ClearanceUtility::RoadmapType

◆ VID

typedef RoadmapType::VID ClearanceUtility::VID

◆ WeightType

typedef MPBaseObject::WeightType ClearanceUtility::WeightType

Constructor & Destructor Documentation

◆ ClearanceUtility() [1/2]

ClearanceUtility::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	_positionalDofsOnly = `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.
_positionalDofsOnly	Use only positional DOFs.
_debug	Set to debug mode or not.

◆ ClearanceUtility() [2/2]

ClearanceUtility::ClearanceUtility ( XMLNode & _node )

Construct a ClearanceUtility object from an XML node

Parameters

_node The XML node to use

◆ ~ClearanceUtility()

virtual ClearanceUtility::~ClearanceUtility ( )

virtualdefault

Member Function Documentation

◆ AdjustWitnessToEnsureValidity()

bool ClearanceUtility::AdjustWitnessToEnsureValidity	(	const Cfg &	_cfg,
		const bool	_initValidity,
		Cfg &	_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. this is a helper function called by ExactCollisionInfo.

Parameters

_cfg	The sampled configuration to we wish to push
_initValidity	The initial validity of _cfg
_witnessCfg	The candidiate witness configuration
_b	The boundary to use
_useOppValidityWitness	Whether _cfg and _witness should be the the same validity or not

Returns: Whether the witness was successfully adjusted to satisfy the _useOppValidityWitness condition. If succesfully, the _witnessCfg is updated.

◆ ApproxCollisionInfo()

bool ClearanceUtility::ApproxCollisionInfo	(	Cfg &	_cfg,
		Cfg &	_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 stored as the witness.

Parameters

_cfg	The configuration to push
_clrCfg	Place holder for the clearance configuration
_b	The boundary
_cdInfo	Collision detector information reference
_useOppValidityWitness	Whether the witness should be of opposite validity to _cfg.

Returns: Whether a witness was found, if found information will be stored in _clrCfg and _cdInfo.

◆ CollisionInfo()

bool ClearanceUtility::CollisionInfo	(	Cfg &	_cfg,
		Cfg &	_clrCfg,
		const Boundary *const	_b,
		CDInfo &	_cdInfo,
		const bool &	_useOppValidityWitness = `true`
	)

Calculate clearance information as well as the closest witness approximately or exactly.

Parameters

_cfg	The configuration to push
_clrCfg	Place holder for the clearance configuration
_b	The boundary
_cdInfo	Collision detector information reference
_useOppValidityWitness	Whether the witness should be of opposite validity to _cfg.

Returns: Whether a witness was found, if found information will be stored in _clrCfg and _cdInfo.

◆ EdgeClearance()

std::vector< double > ClearanceUtility::EdgeClearance	(	const Cfg &	_c1,
		const Cfg &	_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.

◆ ExactCollisionInfo()

bool ClearanceUtility::ExactCollisionInfo	(	Cfg &	_cfg,
		Cfg &	_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.

Parameters

_cfg	The configuration to push
_clrCfg	Place holder for the clearance configuration
_b	The boundary
_cdInfo	Collision detector information reference
_useOppValidityWitness	Whether the witness should be of opposite validity to _cfg.

Returns: Whether a witness was found, if found information will be stored in _clrCfg and _cdInfo.

◆ FindApproximateWitness()

bool ClearanceUtility::FindApproximateWitness	(	const std::size_t &	_numRays,
		std::vector< Ray< Cfg > > &	_rays,
		const Cfg &	_sampledCfg,
		const bool &	_initValidity,
		const Boundary *const	_b,
		std::pair< size_t, Cfg > &	_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.

Parameters

_numRays	The number of rays to search for the nearest witness
_rays	The container for the rays, if empty will be populated
_sampledCfg	The sampled configuration to eventually push
_initValidity	The initial validity of the _sampledCfg
_b	The boundary to use
_candidate	Container for candidate witness : _candidate.first is the index of the candidate ray within _rays and _candidate.second is the candidate configuration.
_useOppValidityWitness	Whether the witness should be of opposite validity to _sampledCfg

Returns: Whether a witness was found, if successful t he witness can be found in _candidate.second

◆ GetDistanceMetricLabel()

const string& ClearanceUtility::GetDistanceMetricLabel ( ) const

inline

Get the label of the distance metric

Returns: The label of the distance metric.

◆ GetExactClearance()

bool ClearanceUtility::GetExactClearance ( ) const

inline

Get whether exact clearance is used or not

Returns: Whether exact clearance is being used or not as a bool.

◆ GetNearestVertexWitness()

const bool ClearanceUtility::GetNearestVertexWitness	(	Cfg &	_cfg,
		CDInfo &	_cdInfo,
		const Boundary *const	_b
	)

This function places the nearest vertex of the environment and the corresponding robot vertex into _cdInfo. This function is only used by the exact version of collision info.

Parameters

_cfg	The sampled configuration to push
_cdInfo	The container for collision detection information. The nearest configuration of the environment (witness candidate) and the point on the robot which corresponds to this config will be stored in _cdInfo if successful.
_b	The boundary to use

Returns: The validity of _cfg provided.

◆ GetOrientationResolution()

double ClearanceUtility::GetOrientationResolution ( ) const

inline

Get the orientation resolution

Returns: The orientation resolution.

◆ GetPositionResolution()

double ClearanceUtility::GetPositionResolution ( ) const

inline

Get the position resolution

Returns: The position resolution.

◆ GetValidityCheckerLabel()

const string& ClearanceUtility::GetValidityCheckerLabel ( ) const

inline

Get the label of the validity checker

Returns: The label of the validity checker.

◆ Initialize()

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

◆ IsInitialized()

bool ClearanceUtility::IsInitialized ( ) const

inline

Get whether the object is initialized or not

Returns: Whether the ClearanceUtility object has been initailized.

◆ MakeRays()

void ClearanceUtility::MakeRays	(	const Cfg &	_sampledCfg,
		const std::size_t &	_numRays,
		std::vector< Ray< Cfg > > &	_rays
	)

Generates the rays for the witness finding. This is a helper function of FindApproximateWitness which will only be called if the _rays parameter passed into FindapproximateWitness is empty.

Parameters

_sampledCfg	The sampled configuration we wish to push
_numRays	The number of rays to search for the nearest witness
_rays	The container for the rays generated.

Returns: The rays for witness exploration are stored in _rays.

@TODO expand to 3D, and then to N dimensions for uniform distribution: there are only approximate algorithms to do this "fib lattices"

◆ PathClearance() [1/2]

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

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

Parameters

_path The path to use.

◆ PathClearance() [2/2]

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

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

Parameters

_path The path to use.

◆ Print()

void ClearanceUtility::Print ( ostream & _os ) const

overridevirtual

Print the internal state of this object

Parameters

_os	The std::ostream to print to.

Reimplemented from MPBaseObject.

Reimplemented in MedialAxisUtility.

◆ RoadmapClearance()

ClearanceStats ClearanceUtility::RoadmapClearance ( )

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

◆ SetRobot()

void ClearanceUtility::SetRobot ( Robot * _robot )

◆ SetValidityCheckerLabel()

void ClearanceUtility::SetValidityCheckerLabel ( const string & _s )

inline

Set the label of the validity checker

Parameters

_s	The label of the validity checker to use.

◆ ValidateCandidate()

bool ClearanceUtility::ValidateCandidate	(	const std::pair< size_t, Cfg > &	_cand,
		const std::vector< Ray< Cfg > > &	_rays,
		const Boundary *const	_b,
		const bool &	_useOppValidityWitness = `true`
	)

A final sanity check to be performed on the witness candidate. Compares the candidate cfg with the "previous" ticked configuration. The appropriate direction of the tick is dependent on whether opposite validity is true or not.

Parameters

_cand	The information for the candidate witness, has two parts. _cand.first is the index of the ray (within _rays) which lead to the candidate witness. _cand.second is the actual configuration of the candidate witness.
_rays	The vector which stores all rays that were explored.
_b	The boundary being used.
useOppValidityWitness	Whether we are asserting that the witness should be of opposite validity to the sampled configuration.

Field Documentation

◆ abcd

int ClearanceUtility::abcd = 0

protected

◆ m_clearanceRays

size_t ClearanceUtility::m_clearanceRays {10}

protected

Number of rays for approximate clearance.

◆ m_dmLabel

string ClearanceUtility::m_dmLabel {"euclidean"}

protected

Distance metric method label.

◆ m_exactClearance

bool ClearanceUtility::m_exactClearance {false}

protected

Use exact clearance calculations?

◆ m_exactPenetration

bool ClearanceUtility::m_exactPenetration {false}

protected

Use exact penetration calculations?

◆ m_initialized

bool ClearanceUtility::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:

◆ m_maSearchResolutionFactor

double ClearanceUtility::m_maSearchResolutionFactor {1}

protected

The search resolution factor.

◆ m_maxRayIterations

size_t ClearanceUtility::m_maxRayIterations

protected

The maximum number of ray iterations.

◆ m_maxRayMagnitude

double ClearanceUtility::m_maxRayMagnitude

protected

How far out a ray will be attempted.

◆ m_orientationResFactor

double ClearanceUtility::m_orientationResFactor {1.}

protected

The orientation resolution factor.

◆ m_orientationResolution

double ClearanceUtility::m_orientationResolution

protected

The orientation resolution.

◆ m_penetrationRays

size_t ClearanceUtility::m_penetrationRays {10}

protected

Number of rays for approximate penetration.

◆ m_positionalDofsOnly

bool ClearanceUtility::m_positionalDofsOnly {true}

protected

Use only positional dofs?

◆ m_positionalResFactor

double ClearanceUtility::m_positionalResFactor {1.}

protected

The position resoluiton factor.

◆ m_rayTickResolution

double ClearanceUtility::m_rayTickResolution

protected

The resolution for rayTicks.

◆ m_robot

Robot* ClearanceUtility::m_robot

protected

◆ m_useBBX

bool ClearanceUtility::m_useBBX {true}

protected

Use bounding box as obstacle?

◆ m_vcLabel

string ClearanceUtility::m_vcLabel {"cd4"}

protected

Validity checker method label.

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

/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/MPTools/ClearanceUtilities.h
/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/MPTools/ClearanceUtilities.cpp

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ RoadmapType

◆ VID

◆ WeightType

Constructor & Destructor Documentation

◆ ClearanceUtility() [1/2]

◆ ClearanceUtility() [2/2]

◆ ~ClearanceUtility()

Member Function Documentation

◆ AdjustWitnessToEnsureValidity()

◆ ApproxCollisionInfo()

◆ CollisionInfo()

◆ EdgeClearance()

◆ ExactCollisionInfo()

◆ FindApproximateWitness()

◆ GetDistanceMetricLabel()

◆ GetExactClearance()

◆ GetNearestVertexWitness()

◆ GetOrientationResolution()

◆ GetPositionResolution()

◆ GetValidityCheckerLabel()

◆ Initialize()

◆ IsInitialized()

◆ MakeRays()

◆ PathClearance() [1/2]

◆ PathClearance() [2/2]

◆ Print()

◆ RoadmapClearance()

◆ SetRobot()

◆ SetValidityCheckerLabel()

◆ ValidateCandidate()

Field Documentation

◆ abcd

◆ m_clearanceRays

◆ m_dmLabel

◆ m_exactClearance

◆ m_exactPenetration

◆ m_initialized

◆ m_maSearchResolutionFactor

◆ m_maxRayIterations

◆ m_maxRayMagnitude

◆ m_orientationResFactor

◆ m_orientationResolution

◆ m_penetrationRays

◆ m_positionalDofsOnly

◆ m_positionalResFactor

◆ m_rayTickResolution

◆ m_robot

◆ m_useBBX

◆ m_vcLabel