#include <CollisionDetectionValidity.h>

Inheritance diagram for CollisionDetectionValidity:

[legend]

Collaboration diagram for CollisionDetectionValidity:

[legend]

Public Types
Local Types
typedef CollisionDetectionValidityMethod::GroupCfg	GroupCfg

typedef GroupCfg::Formation	Formation

Public Types inherited from CollisionDetectionValidityMethod
typedef ValidityCheckerMethod::GroupCfgType	GroupCfg

Public Types inherited from ValidityCheckerMethod
typedef MPBaseObject::GroupCfgType	GroupCfgType

typedef GroupCfgType::Formation	Formation

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

	CollisionDetectionValidity (XMLNode &_node)

virtual	~CollisionDetectionValidity ()

CollisionDetection Interface
virtual CollisionDetectionMethod *	GetCDMethod () const noexcept override

virtual bool	IsInsideObstacle (const Point3d &_p) override

virtual bool	IsInsideObstacle (const Point3d &_p, std::vector< size_t > *_obstIdxs)

virtual bool	WorkspaceVisibility (const Point3d &_a, const Point3d &_b) override

virtual bool	IsMultiBodyCollision (CDInfo &_cdInfo, const MultiBody const _a, const MultiBody const _b, const std::string &_caller) override

Public Member Functions inherited from CollisionDetectionValidityMethod
	CollisionDetectionValidityMethod ()=default

	CollisionDetectionValidityMethod (XMLNode &_node)

virtual	~CollisionDetectionValidityMethod ()=default

Public Member Functions inherited from ValidityCheckerMethod
	ValidityCheckerMethod ()=default

	ValidityCheckerMethod (XMLNode &_node)

virtual	~ValidityCheckerMethod ()=default

bool	GetValidity () const
	Get the current meaning of "valid" (true is default). More...

void	ToggleValidity ()
	Switches the meaning of "valid" to "invalid" and vice versa. More...

bool	IsValid (Cfg &_cfg, CDInfo &_cdInfo, const std::string &_caller)

bool	IsValid (Cfg &_cfg, const std::string &_caller)

bool	IsValid (GroupCfgType &_cfg, CDInfo &_cdInfo, const std::string &_caller)

bool	IsValid (GroupCfgType &_cfg, const std::string &_caller)

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

	MPBaseObject (XMLNode &_node)

virtual	~MPBaseObject ()

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

virtual void	Initialize ()

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
ValidityCheckerMethod Overrides
virtual bool	IsValidImpl (Cfg &_cfg, CDInfo &_cdInfo, const std::string &_caller) override

virtual bool	IsValidImpl (GroupCfg &_cfg, CDInfo &_cdInfo, const std::string &_caller) override

Helpers
virtual bool	IsInCollision (CDInfo &_cdInfo, const Cfg &_cfg, const std::string &_caller)

virtual bool	IsInCollision (CDInfo &_cdInfo, const GroupCfg &_cfg, const std::string &_caller)

virtual bool	IsInSelfCollision (CDInfo &_cdInfo, const MultiBody *const _multibody, const std::string &_caller)

virtual bool	IsInBoundaryCollision (CDInfo &_cdInfo, const Cfg &_cfg)

virtual bool	IsInObstacleCollision (CDInfo &_cdInfo, const MultiBody *const _multibody, const std::string &_caller)

virtual bool	IsInInterRobotCollision (CDInfo &_cdInfo, Robot const _robot, const std::vector< Robot > &_robots, const std::string &_caller)

virtual bool	setObstacleSubset (const std::vector< MultiBody * > &_obstacleSubset)

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

const std::string &	GetBaseFilename () const

Protected Attributes
Internal State
std::unique_ptr< CollisionDetectionMethod >	m_cdMethod
	< Underlying collision detection object. More...

bool	m_ignoreSelfCollision {false}
	Check self collisions. More...

bool	m_interRobotCollision {false}
	Check inter-robot collisions. More...

bool	m_ignoreAdjacentLinks {false}
	Ignore adj links in self collisions. More...

bool	m_ignoreSiblingCollisions {false}
	Ignore sibling links in self collisions. More...

std::vector< MultiBody * >	m_obstacleSubset
	Subset of obstacles to check for. More...

Protected Attributes inherited from ValidityCheckerMethod
bool	m_validity {true}
	Use standard validity? False indicates negation. More...

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

Detailed Description

Classifies validity based on collisions with other objects in the workspace.

There are three types of collision that can occur:

Self collision. Two or more pieces of the robot's geometry are overlapping in workspace.
Obstacle collision. The robot's geometry overlaps with some workspace obstacle.
Inter-robot collision. The robot's geometry overlaps with some other robot in its current configuration. For group configurations, collisions between robots within the group are considered as self-collisions, while collisions with robots not in the group are classified as inter-robot collisions. This distinction is made because considering self-collisions usually means that we want (group) configurations which are valid without considering the obstacles and other robots.

Any robot can be omitted from the collision checks by setting it as virtual. When collision checking a virtual robot, other robots are also ignored.

Note: This class interfaces with external CD libraries to determine collision information, sometimes including clearance and penetration information.

Todo:: Remove the 'GetCDMethod' function after re-implementing ObstacleClearanceValidity as a subtype of CollisionDetectionValidity.

Member Typedef Documentation

◆ Formation

typedef GroupCfg::Formation CollisionDetectionValidity::Formation

◆ GroupCfg

typedef CollisionDetectionValidityMethod::GroupCfg CollisionDetectionValidity::GroupCfg

Constructor & Destructor Documentation

◆ CollisionDetectionValidity() [1/2]

CollisionDetectionValidity::CollisionDetectionValidity ( )

◆ CollisionDetectionValidity() [2/2]

CollisionDetectionValidity::CollisionDetectionValidity ( XMLNode & _node )

◆ ~CollisionDetectionValidity()

CollisionDetectionValidity::~CollisionDetectionValidity ( )

virtualdefault

Member Function Documentation

◆ GetCDMethod()

CollisionDetectionMethod * CollisionDetectionValidity::GetCDMethod ( ) const

overridevirtualnoexcept

Returns: Collision Detection object

Reimplemented from CollisionDetectionValidityMethod.

◆ IsInBoundaryCollision()

bool CollisionDetectionValidity::IsInBoundaryCollision	(	CDInfo &	_cdInfo,
		const Cfg &	_cfg
	)

protectedvirtual

Check if any of the robot's bodies are in collision with or outside the environment boundary.

Parameters

_cdInfo	Output for collision detection info. It will only be updated if the detected collision is closer than the previous.
_cfg	The robot configuration.

Returns: True if the robot is in self-collision.

Todo:: This also doesn't work, we can't distinguish between no collision, self collision, and boundary collision since we use -1 for all three.

◆ IsInCollision() [1/2]

bool CollisionDetectionValidity::IsInCollision	(	CDInfo &	_cdInfo,
		const Cfg &	_cfg,
		const std::string &	_caller
	)

protectedvirtual

Orchestrate collision computation between robot and environment multibodies

Parameters

_cdInfo	Output for collision detection info.
_cfg	Configuration of interest.
_caller	Name of the calling function.

Returns: True if the robot is in collision at _cfg.

◆ IsInCollision() [2/2]

bool CollisionDetectionValidity::IsInCollision	(	CDInfo &	_cdInfo,
		const GroupCfg &	_cfg,
		const std::string &	_caller
	)

protectedvirtual

Orchestrate collision computation between robots in a group cfg and environment multibodies

Parameters

_cdInfo	Output for collision detection info.
_cfg	Group configuration of interest.
_caller	Name of the calling function.

Returns: True if the robot group is in collision at _cfg.

◆ IsInInterRobotCollision()

bool CollisionDetectionValidity::IsInInterRobotCollision	(	CDInfo &	_cdInfo,
		Robot *const	_robot,
		const std::vector< Robot * > &	_robots,
		const std::string &	_caller
	)

protectedvirtual

Check for a collision between a query robot and a specific set of other robots.

Parameters

_robot	The query robot.
_robots	The other robots.
_caller	Name of the calling function.

Todo:: Arg, more ambiguous data. See above.

◆ IsInObstacleCollision()

bool CollisionDetectionValidity::IsInObstacleCollision	(	CDInfo &	_cdInfo,
		const MultiBody *const	_multibody,
		const std::string &	_caller
	)

protectedvirtual

Check if any of the robot's bodies are in collision with an obstacle.

Parameters

_cdInfo	Output for collision detection info. It will only be updated if the detected collision is closer than the previous.
_multibody	The robot's MultiBody.
_caller	Name of the calling function.

Returns: True if the robot is in collision with an obstacle.

◆ IsInSelfCollision()

bool CollisionDetectionValidity::IsInSelfCollision	(	CDInfo &	_cdInfo,
		const MultiBody *const	_multibody,
		const std::string &	_caller
	)

protectedvirtual

Check if any of the robot's bodies are in collision with each other.

Parameters

_cdInfo	Output for collision detection info. It will only be updated if the detected collision is closer than the previous.
_multibody	The robot's MultiBody.
_caller	Name of the calling function.

Returns: True if the robot is in self-collision.

Todo:: -1 is the default value here, so this doesn't really work. We need to expand our CDInfo object to handle this properly.

◆ IsInsideObstacle() [1/2]

bool CollisionDetectionValidity::IsInsideObstacle ( const Point3d & _p )

overridevirtual

Determine whether a workspace point lies inside of an obstacle.

Parameters

_p	The workspace point.

Returns: True if _p is inside an obstacle.

Todo:: Implement a bounding box check (per multibody and body) before calling m_cdMethod.

Implements CollisionDetectionValidityMethod.

◆ IsInsideObstacle() [2/2]

bool CollisionDetectionValidity::IsInsideObstacle	(	const Point3d &	_p,
		std::vector< size_t > *	_obstIdxs
	)

virtual

Determine whether a workspace point lies inside of an obstacle from a specific subset of obstacles

Parameters

_p	The workspace point.
_obstIdxs	The indices of the subset of obstacles

Returns: True if _p is inside an obstacle.

Todo:: Implement a bounding box check (per multibody and body) before calling m_cdMethod.

Implements CollisionDetectionValidityMethod.

◆ IsMultiBodyCollision()

bool CollisionDetectionValidity::IsMultiBodyCollision	(	CDInfo &	_cdInfo,
		const MultiBody *const	_a,
		const MultiBody *const	_b,
		const std::string &	_caller
	)

overridevirtual

Check for collision between two multibodies.

Parameters

_cdInfo	CDInfo
_a	The first multibody.
_b	The second multibody.
_caller	Function calling validity checker.

Returns: True if _a and _b collide in their present configurations.

Implements CollisionDetectionValidityMethod.

◆ IsValidImpl() [1/2]

bool CollisionDetectionValidity::IsValidImpl	(	Cfg &	_cfg,
		CDInfo &	_cdInfo,
		const std::string &	_caller
	)

overrideprotectedvirtual

Implementation of the classification of a configuration to either cfree or cobst.

Parameters

_cfg	The individual configuration.
_cdInfo	Output for extra computed information such as clearance.
_caller	Name of the calling function.

Returns: True if _cfg is in cfree.

Implements ValidityCheckerMethod.

◆ IsValidImpl() [2/2]

bool CollisionDetectionValidity::IsValidImpl	(	GroupCfg &	_cfg,
		CDInfo &	_cdInfo,
		const std::string &	_caller
	)

overrideprotectedvirtual

Implementation of group cfg classification.

Parameters

_cfg	The group configuration.
_cdInfo	Output for extra computed information such as clearance.
_caller	Name of the calling function.

Returns: True if _cfg is in cfree.

Reimplemented from ValidityCheckerMethod.

◆ setObstacleSubset()

bool CollisionDetectionValidity::setObstacleSubset ( const std::vector< MultiBody * > & _obstacleSubset )

protectedvirtual

Sets member variable m_obstacleSubset Once this is set, isInObstacleCollision will only check the obstacles in this array for collision with the robot. If it is not set or empty it will check for collision with all obstacles in the environment

Parameters

_obstacleSubset A subset of obstacle multibodies in the environment

◆ WorkspaceVisibility()

bool CollisionDetectionValidity::WorkspaceVisibility	(	const Point3d &	_a,
		const Point3d &	_b
	)

overridevirtual

Check if two workspace points are mutually visible.

Parameters

_a	The first point.
_b	The second point.

Returns: True if _a is visible from _b and vice versa.

Implements CollisionDetectionValidityMethod.

Field Documentation

◆ m_cdMethod

std::unique_ptr<CollisionDetectionMethod> CollisionDetectionValidity::m_cdMethod

protected

< Underlying collision detection object.

◆ m_ignoreAdjacentLinks

bool CollisionDetectionValidity::m_ignoreAdjacentLinks {false}

protected

Ignore adj links in self collisions.

◆ m_ignoreSelfCollision

bool CollisionDetectionValidity::m_ignoreSelfCollision {false}

protected

Check self collisions.

◆ m_ignoreSiblingCollisions

bool CollisionDetectionValidity::m_ignoreSiblingCollisions {false}

protected

Ignore sibling links in self collisions.

◆ m_interRobotCollision

bool CollisionDetectionValidity::m_interRobotCollision {false}

protected

Check inter-robot collisions.

◆ m_obstacleSubset

std::vector<MultiBody*> CollisionDetectionValidity::m_obstacleSubset

protected

Subset of obstacles to check for.

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

/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/ValidityCheckers/CollisionDetectionValidity.h
/opt/actions-runner/_work/open-ppl/open-ppl/src/MPLibrary/ValidityCheckers/CollisionDetectionValidity.cpp

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ Formation

◆ GroupCfg

Constructor & Destructor Documentation

◆ CollisionDetectionValidity() [1/2]

◆ CollisionDetectionValidity() [2/2]

◆ ~CollisionDetectionValidity()

Member Function Documentation

◆ GetCDMethod()

◆ IsInBoundaryCollision()

◆ IsInCollision() [1/2]

◆ IsInCollision() [2/2]

◆ IsInInterRobotCollision()

◆ IsInObstacleCollision()

◆ IsInSelfCollision()

◆ IsInsideObstacle() [1/2]

◆ IsInsideObstacle() [2/2]

◆ IsMultiBodyCollision()

◆ IsValidImpl() [1/2]

◆ IsValidImpl() [2/2]

◆ setObstacleSubset()

◆ WorkspaceVisibility()

Field Documentation

◆ m_cdMethod

◆ m_ignoreAdjacentLinks

◆ m_ignoreSelfCollision

◆ m_ignoreSiblingCollisions

◆ m_interRobotCollision

◆ m_obstacleSubset