#include <GroupCfg.h>

Inheritance diagram for GroupCfg< GraphType >:

Collaboration diagram for GroupCfg< GraphType >:

[legend]

Public Types
Local Types
typedef CompositeState< GraphType >	BaseType

typedef GroupRoadmap< GroupCfg, GroupLocalPlan< GraphType > >	GroupRoadmapType

typedef BaseType::VID	VID

typedef BaseType::GroupGraphType	GroupGraphType

typedef BaseType::CfgType	IndividualCfg

typedef std::vector< size_t >	Formation

Public Types inherited from CompositeState< GraphType >
typedef size_t	VID
	A VID in an individual graph. More...

typedef std::vector< VID >	VIDSet
	A set of VIDs from indiv. graphs. More...

typedef GraphType::CfgType	CfgType
	The indiv. graph vertex type. More...

typedef CompositeEdge< GraphType >	CompositeEdgeType

typedef CompositeGraph< CompositeState, CompositeEdgeType >	GroupGraphType

typedef GraphType	IndividualGraph

Public Member Functions
Construction
	GroupCfg (GroupRoadmapType *const &_groupMap=nullptr)

	GroupCfg (RobotGroup *const &_group)

Equality
bool	operator== (const GroupCfg &_other) const noexcept

bool	operator< (const GroupCfg &_other) const noexcept

Arithmetic
GroupCfg	operator+ (const GroupCfg &_other) const

GroupCfg	operator- (const GroupCfg &_other) const

GroupCfg	operator* (const double &_other) const

GroupCfg &	operator+= (const GroupCfg &_other)

GroupCfg &	operator-= (const GroupCfg &_other)

GroupCfg &	operator*= (const double &_val)

Roadmap Accessors
These functions provide access to the related group map (if any) and descriptors for non-local individual configurations.
GroupRoadmapType *	GetGroupRoadmap () const noexcept
	Get the group roadmap this group cfg is with respect to. More...

void	SetGroupRoadmap (GroupRoadmapType *const _newRoadmap)

DOF Accessors
bool	IsNonholonomic () const noexcept
	Check if there is a nonholonomic robot in the group. More...

size_t	CompositeDOF () const
	Compute the total DOF for this robot group. More...

double	Magnitude () const
	Compute the composite magnitude. More...

double	PositionMagnitude () const
	Compute the composite manitude of the positional components. More...

double	OrientationMagnitude () const
	Compute the composite magnitude of the orientation components. More...

Configuration Helpers
void	ConfigureRobot () const

bool	WithinResolution (const GroupCfg &_cfg, const double _posRes, const double _oriRes) const

DOF Modifiers
void	RotateFormationAboutLeader (const Formation &_robotList, const mathtool::Orientation &_rotation, const bool _debug=false)

void	ApplyTransformationForRobots (const Formation &_robotList, const mathtool::Transformation &_transform, const mathtool::Transformation &_relativeTransform=mathtool::Transformation())

void	AddDofsForRobots (const std::vector< double > &_dofs, const Formation &_robots)

void	AddDofsForRobots (const mathtool::Vector3d &_dofs, const Formation &_robots)

void	OverwriteDofsForRobots (const std::vector< double > &_dofs, const Formation &_robots)

void	OverwriteDofsForRobots (const mathtool::Vector3d &_dofs, const Formation &_robots)

void	OverwriteDofsForRobots (const GroupCfg &_fromCfg, const Formation &_robots)

void	OverwriteDofsForRobots (const GroupCfg &_fromCfg, const std::vector< Robot * > &_robots)

void	SetData (const std::vector< double > &_dofs)

void	FindIncrement (const GroupCfg &_start, const GroupCfg &_goal, const int _nTicks)

void	FindIncrement (const GroupCfg &_start, const GroupCfg &_goal, int *const _nTicks, const double _positionRes, const double _orientationRes)

bool	InBounds (const Boundary *const _b) const noexcept

bool	InBounds (const Environment *const _env) const noexcept
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More...

void	GetRandomGroupCfg (const Boundary *const _b)

void	GetRandomGroupCfg (Environment *_env)

template<typename DistanceMetricPointer >
void	GetRandomRay (const double _length, DistanceMetricPointer _dm, const bool _norm=true)

virtual void	NormalizeOrientation (const std::vector< size_t > &_robots={}) noexcept
	Normalize Orientation DOFs for a Group Cfg. More...

Output Utilities
std::string	PrettyPrint (const size_t _precision=4) const

Public Member Functions inherited from CompositeState< GraphType >
	CompositeState (GroupGraphType *const _groupGraph=nullptr)

	CompositeState (RobotGroup *const _group)

virtual bool	operator== (const CompositeState &_other) const noexcept

virtual bool	operator!= (const CompositeState &_other) const noexcept

virtual size_t	GetNumRobots () const noexcept
	Get the number of robots in this composite state. More...

virtual const std::vector< Robot * > &	GetRobots () const noexcept
	Get the full vector of robot pointers. More...

virtual Robot *	GetRobot (const size_t _index) const

virtual GroupGraphType *	GetGroupGraph () const noexcept
	Get the group graph this composite state is with respect to. More...

virtual void	SetGroupGraph (GroupGraphType *_newGraph)
	Set the composite graph that this composite state exists within. More...

virtual VID	GetVID (const size_t _index) const noexcept

virtual VID	GetVID (Robot *const _robot) const

virtual void	SetRobotCfg (Robot *const _robot, const VID _vid)

virtual void	SetRobotCfg (const size_t _index, const VID _vid)

virtual void	SetRobotCfg (Robot *const _robot, CfgType &&_cfg)

virtual void	SetRobotCfg (const size_t _index, CfgType &&_cfg)

virtual CfgType &	GetRobotCfg (Robot *const _robot)

virtual CfgType &	GetRobotCfg (const size_t _index)

virtual const CfgType &	GetRobotCfg (Robot *const _robot) const

virtual const CfgType &	GetRobotCfg (const size_t _index) const

void	ClearLocalCfgs ()
	Clear the Local Cfg information. More...

Additional Inherited Members
Protected Member Functions inherited from CompositeState< GraphType >
virtual void	VerifyIndex (const size_t _robotIndex) const noexcept

virtual bool	IsLocalCfg (const size_t _robotIndex) const noexcept

Protected Attributes inherited from CompositeState< GraphType >
GroupGraphType *	m_groupMap {nullptr}
	The group graph. More...

RobotGroup *	m_group {nullptr}
	The robot group for this state. More...

VIDSet	m_vids
	The individual VIDs in this aggregate state. More...

std::vector< CfgType >	m_localCfgs
	Individual states not in a map. More...

Detailed Description

template<typename GraphType>
class GroupCfg< GraphType >

An aggregate configuration which represents a configuration for each robot in a robot group.

The main point of group cfg is to take advantage of everything implemented for individual robots. This means that we use VIDs from individual roadmaps to keep track of the robot cfgs referred to in a group cfg. In the case that a group cfg is modified or new in some way, there is a temporary local storage (m_localCfgs) which stores individual cfgs not yet in a roadmap. When adding a group cfg to a group roadmap, the VID is used in place after adding the individual cfg to the individual roadmap.

'GraphType' represents the individual roadmap type for a single robot.

Member Typedef Documentation

◆ BaseType

template<typename GraphType >

typedef CompositeState<GraphType> GroupCfg< GraphType >::BaseType

◆ Formation

template<typename GraphType >

typedef std::vector<size_t> GroupCfg< GraphType >::Formation

A formation represents a group of robots which are maintaining their configurations relative to a leader, such as maintaining a square or V-shape while moving. The values are robot indexes (within the group, not problem) with the first index denoting the leader robot. These are not stored in configurations but may be required for edges.

◆ GroupGraphType

template<typename GraphType >

typedef BaseType::GroupGraphType GroupCfg< GraphType >::GroupGraphType

◆ GroupRoadmapType

template<typename GraphType >

typedef GroupRoadmap<GroupCfg, GroupLocalPlan<GraphType> > GroupCfg< GraphType >::GroupRoadmapType

◆ IndividualCfg

template<typename GraphType >

typedef BaseType::CfgType GroupCfg< GraphType >::IndividualCfg

◆ VID

template<typename GraphType >

typedef BaseType::VID GroupCfg< GraphType >::VID

Constructor & Destructor Documentation

◆ GroupCfg() [1/2]

template<typename GraphType >

GroupCfg< GraphType >::GroupCfg ( GroupRoadmapType *const & _groupMap = nullptr )

explicit

Construct a group configuration.

Parameters

_groupMap The group roadmap to which this configuration belongs, or null if it is not in a map.

◆ GroupCfg() [2/2]

template<typename GraphType >

GroupCfg< GraphType >::GroupCfg ( RobotGroup *const & _group )

explicit

Construct a group configuration.

Parameters

_group The group to which this configuration belongs.

Member Function Documentation

◆ AddDofsForRobots() [1/2]

template<typename GraphType >

void GroupCfg< GraphType >::AddDofsForRobots	(	const mathtool::Vector3d &	_dofs,
		const Formation &	_robots
	)

This function adds all positional dofs in _dofs. It will handle 1-3 dofs based on each IndividualCfg's PosDof value. Does not add in orientation. Note: This function is slightly more efficient than the std::vector version, as we do not need to check the size of _dofs.

Parameters

_dofs	The positional values to add in to each body.
_robots	This list of bodies to update. Order doesn't matter.

◆ AddDofsForRobots() [2/2]

template<typename GraphType >

void GroupCfg< GraphType >::AddDofsForRobots	(	const std::vector< double > &	_dofs,
		const Formation &	_robots
	)

Given this configuration, add in the same DOF values to each body given. It assumes that all robots in the formation given have the same DOFs. This is a common thing to do in assembly planning/composite C-Spaces.

Parameters

_dofs	The values to add in to each body. This function assumes each body has #dofs = _dofs.size().
_robots	This list of bodies to update. Order doesn't matter.

◆ ApplyTransformationForRobots()

template<typename GraphType >

void GroupCfg< GraphType >::ApplyTransformationForRobots	(	const Formation &	_robotList,
		const mathtool::Transformation &	_transform,
		const mathtool::Transformation &	_relativeTransform = `mathtool::Transformation()`
	)

Given this GroupCfg as the starting configuration, this function applies a transformation uniformly over all robots listed. Note: Currently assumes all robots just have ONE body. The case of multi- bodied robots would need to be specially handled (right now it should just be split into multiple robots if a group is needed). Note: This is assuming 6 DOFs!

Parameters

_robotList	This list of bodies to rotate. First one is leader body.
_transform	The change in orientation that should be applied to _cfg.
_relativeTransform	(Optional) The transformation to "undo" before applying _transform. If default, it will be a simple transform application. See RotateFormationAboutLeader for usage example.

Todo:: Generalize this to handle robots with more than one body.

◆ CompositeDOF()

template<typename GraphType >

size_t GroupCfg< GraphType >::CompositeDOF

Compute the total DOF for this robot group.

◆ ConfigureRobot()

template<typename GraphType >

void GroupCfg< GraphType >::ConfigureRobot

Configure each individual cfg that this group cfg represents. This is for template cooperation and is also a needed function before dealing with CD calls and such. Note this will throw an exception if no cfg is present for any robot.

◆ FindIncrement() [1/2]

template<typename GraphType >

void GroupCfg< GraphType >::FindIncrement	(	const GroupCfg< GraphType > &	_start,
		const GroupCfg< GraphType > &	_goal,
		const int	_nTicks
	)

Find the c-space increment and number of steps needed to move from a start to a goal, taking steps no larger than the designated resolutions.

Parameters

_start	The start configuration.
_goal	The goal configuration.
_nTicks	The number of steps to take (NOT computed by this method)

◆ FindIncrement() [2/2]

template<typename GraphType >

void GroupCfg< GraphType >::FindIncrement	(	const GroupCfg< GraphType > &	_start,
		const GroupCfg< GraphType > &	_goal,
		int *const	_nTicks,
		const double	_positionRes,
		const double	_orientationRes
	)

Find the c-space increment and number of steps needed to move from a start to a goal, taking steps no larger than the designated resolutions.

Parameters

_start	The start configuration.
_goal	The goal configuration.
_nTicks	The number of steps to take (computed by this method).
_positionRes	The position resolution to use.
_orientationRes	The orientation resolution to use.

◆ GetGroupRoadmap()

template<typename GraphType >

GroupCfg< GraphType >::GroupRoadmapType * GroupCfg< GraphType >::GetGroupRoadmap

noexcept

Get the group roadmap this group cfg is with respect to.

◆ GetRandomGroupCfg() [1/2]

template<typename GraphType >

void GroupCfg< GraphType >::GetRandomGroupCfg ( const Boundary *const _b )

Create a group configuration where every vertex of every robots is guaranteed to lie within the specified boundary. If a group cfg cannot be found, the program will abort. The function will try a predefined number of times.

Parameters

_b	The bondary to sample within.

◆ GetRandomGroupCfg() [2/2]

template<typename GraphType >

void GroupCfg< GraphType >::GetRandomGroupCfg ( Environment * _env )

Create a group cfg where all robots are guaranteed to lie within the input environment.

Parameters

_env	The environment to sample within.

◆ GetRandomRay()

template<typename GraphType >

template<class DistanceMetricPointer >

void GroupCfg< GraphType >::GetRandomRay	(	const double	_length,
		DistanceMetricPointer	_dm,
		const bool	_norm = `true`
	)

Generate a random configuration for group robotswith a set length.

Parameters

_length	The desired length.
_dm	The distance metric for checking length.
_norm	Normalize the orientation DOFs?

◆ InBounds() [1/2]

template<typename GraphType >

bool GroupCfg< GraphType >::InBounds ( const Boundary *const _b ) const

noexcept

Test if a group configuration lies within a boundary and also within the robot's c-space limits.

Parameters

_boundary The boundary to check.

Returns: True if the configuration places the robot inside both the boundary and its DOF limits.

◆ InBounds() [2/2]

template<typename GraphType >

bool GroupCfg< GraphType >::InBounds ( const Environment *const _env ) const

noexcept

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

◆ IsNonholonomic()

template<typename GraphType >

bool GroupCfg< GraphType >::IsNonholonomic

noexcept

Check if there is a nonholonomic robot in the group.

◆ Magnitude()

template<typename GraphType >

double GroupCfg< GraphType >::Magnitude

Compute the composite magnitude.

◆ NormalizeOrientation()

template<typename GraphType >

void GroupCfg< GraphType >::NormalizeOrientation ( const std::vector< size_t > & _robots = {} )

virtualnoexcept

Normalize Orientation DOFs for a Group Cfg.

◆ operator*()

template<typename GraphType >

GroupCfg< GraphType > GroupCfg< GraphType >::operator* ( const double & _other ) const

Find the product of the current and a given group configuration, by each degree of freedom.

Parameters

_other The group configuration used to multiply the current.

Returns: The product of the group configurations.

◆ operator*=()

template<typename GraphType >

GroupCfg< GraphType > & GroupCfg< GraphType >::operator*= ( const double & _val )

Multiply the current group configuration by a scalar.

Parameters

_val	The scalar used to multiply the current.

◆ operator+()

template<typename GraphType >

GroupCfg< GraphType > GroupCfg< GraphType >::operator+ ( const GroupCfg< GraphType > & _other ) const

Find the sum of the current and a given group configuration, by each degree of freedom.

Parameters

_other The group configuration to be added.

Returns: The sum of the group configurations.

◆ operator+=()

template<typename GraphType >

GroupCfg< GraphType > & GroupCfg< GraphType >::operator+= ( const GroupCfg< GraphType > & _other )

Add a given group configuration to the current, by each degree of freedom.

Parameters

_other The group configuration to be added.

◆ operator-()

template<typename GraphType >

GroupCfg< GraphType > GroupCfg< GraphType >::operator- ( const GroupCfg< GraphType > & _other ) const

Find the difference of the current and a given group configuration, by each degree of freedom.

Parameters

_other The group configuration to be subtracted.

Returns: The difference of the group configurations.

◆ operator-=()

template<typename GraphType >

GroupCfg< GraphType > & GroupCfg< GraphType >::operator-= ( const GroupCfg< GraphType > & _other )

Subtract a given group configuration from the current, by each degree of freedom.

Parameters

_other The group configuration to be subtracted.

◆ operator<()

template<typename GraphType >

bool GroupCfg< GraphType >::operator< ( const GroupCfg< GraphType > & _other ) const

noexcept

Check if the current group cfg is less than the given group cfg.

Parameters

_other The given group configuration.

Returns: True if less than, false otherwise.

◆ operator==()

template<typename GraphType >

bool GroupCfg< GraphType >::operator== ( const GroupCfg< GraphType > & _other ) const

noexcept

Check if the current and given group configurations are equal.

Parameters

_other The given group configuration.

Returns: True if equal, false otherwise.

◆ OrientationMagnitude()

template<typename GraphType >

double GroupCfg< GraphType >::OrientationMagnitude

Compute the composite magnitude of the orientation components.

◆ OverwriteDofsForRobots() [1/4]

template<typename GraphType >

void GroupCfg< GraphType >::OverwriteDofsForRobots	(	const GroupCfg< GraphType > &	_fromCfg,
		const Formation &	_robots
	)

Given this and another configuration, copy the DOF values from the other to the DOF values in this one, but only for each given robot indexed.

Parameters

_fromCfg	The configuration to take values from.
_robots	This list of bodies to update. Order doesn't matter.

◆ OverwriteDofsForRobots() [2/4]

template<typename GraphType >

void GroupCfg< GraphType >::OverwriteDofsForRobots	(	const GroupCfg< GraphType > &	_fromCfg,
		const std::vector< Robot * > &	_robots
	)

◆ OverwriteDofsForRobots() [3/4]

template<typename GraphType >

void GroupCfg< GraphType >::OverwriteDofsForRobots	(	const mathtool::Vector3d &	_dofs,
		const Formation &	_robots
	)

Given new DOF values, overwrite the existing values for each individual cfg in this group cfg that is listed in _robots. Note that _dofs needs to be the same number of DOFs as each individual cfg in the group.

Parameters

_fromCfg	The configuration to take values from.
_robots	This list of bodies to update. Order doesn't matter.

◆ OverwriteDofsForRobots() [4/4]

template<typename GraphType >

void GroupCfg< GraphType >::OverwriteDofsForRobots	(	const std::vector< double > &	_dofs,
		const Formation &	_robots
	)

Given new DOF values, overwrite the existing values for each individual cfg in this group cfg that is listed in _robots. Note that _dofs needs to be the same number of DOFs as each individual cfg in the group.

Parameters

_fromCfg	The configuration to take values from.
_robots	This list of bodies to update. Order doesn't matter.

◆ PositionMagnitude()

template<typename GraphType >

double GroupCfg< GraphType >::PositionMagnitude

Compute the composite manitude of the positional components.

◆ PrettyPrint()

template<typename GraphType >

std::string GroupCfg< GraphType >::PrettyPrint ( const size_t _precision = 4 ) const

◆ RotateFormationAboutLeader()

template<typename GraphType >

void GroupCfg< GraphType >::RotateFormationAboutLeader	(	const Formation &	_robotList,
		const mathtool::Orientation &	_rotation,
		const bool	_debug = `false`
	)

Given this GroupCfg as the starting configuration, this function applies a rotation to all the robots that are listed, assuming the first one is the formation's leader. Note: Currently assumes all robots just have ONE body. The case of multi- bodied robots would need to be specially handled (right now it should just be split into multiple robots if a group is needed).

Parameters

_robotList	This list of bodies to rotate. First one is leader body.
_rotation	The change in orientation that should be applied to _cfg.
_debug	A flag to print to cout (no access to an m_debug flag here).

Note: Currently assumes all robots just have ONE body. The case of multi- bodied robots would need to be specially handled (right now it should just be split into multiple robots if a group is needed).

Todo:: We can probably compute this without having to configure the models (which cost a lot of transformations).

◆ SetData()

template<typename GraphType >

void GroupCfg< GraphType >::SetData ( const std::vector< double > & _dofs )

Overwrites all data in this cfg, assumes the length of _dofs is the same as CompositeDOF(). Basically just converts a composite C-Space vector into its individual pieces for a Group Cfg.

◆ SetGroupRoadmap()

template<typename GraphType >

void GroupCfg< GraphType >::SetGroupRoadmap ( GroupRoadmapType *const _newRoadmap )

Change the roadmap that this group is using/in reference to. Also performs compatibility/verification tests to see if it's possible.

Note: Does NOT add this new cfg to any roadmap, but makes all cfg info local (everything will be in m_localCfgs) so that there are no issues when adding to the roadmap later.

◆ WithinResolution()

template<typename GraphType >

bool GroupCfg< GraphType >::WithinResolution	(	const GroupCfg< GraphType > &	_cfg,
		const double	_posRes,
		const double	_oriRes
	)		const

Check that another GroupCfg is within a resolution as specified.

Parameters

_cfg	The test configuration.
_posRes	The position resolution.
_oriRes	The orientation resolution.

Returns: True if each individual configuration in this is within a resolution distance of _cfg.

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

/opt/actions-runner/_work/open-ppl/open-ppl/src/ConfigurationSpace/GroupCfg.h

Public Types

Public Member Functions

Additional Inherited Members

Detailed Description

template<typename GraphType> class GroupCfg< GraphType >

Member Typedef Documentation

◆ BaseType

◆ Formation

◆ GroupGraphType

◆ GroupRoadmapType

◆ IndividualCfg

◆ VID

Constructor & Destructor Documentation

◆ GroupCfg() [1/2]

◆ GroupCfg() [2/2]

Member Function Documentation

◆ AddDofsForRobots() [1/2]

◆ AddDofsForRobots() [2/2]

◆ ApplyTransformationForRobots()

◆ CompositeDOF()

◆ ConfigureRobot()

◆ FindIncrement() [1/2]

◆ FindIncrement() [2/2]

◆ GetGroupRoadmap()

◆ GetRandomGroupCfg() [1/2]

◆ GetRandomGroupCfg() [2/2]

◆ GetRandomRay()

◆ InBounds() [1/2]

◆ InBounds() [2/2]

◆ IsNonholonomic()

◆ Magnitude()

◆ NormalizeOrientation()

◆ operator*()

◆ operator*=()

◆ operator+()

◆ operator+=()

◆ operator-()

◆ operator-=()

◆ operator<()

◆ operator==()

◆ OrientationMagnitude()

◆ OverwriteDofsForRobots() [1/4]

◆ OverwriteDofsForRobots() [2/4]

◆ OverwriteDofsForRobots() [3/4]

◆ OverwriteDofsForRobots() [4/4]

◆ PositionMagnitude()

◆ PrettyPrint()

◆ RotateFormationAboutLeader()

◆ SetData()

◆ SetGroupRoadmap()

◆ WithinResolution()

template<typename GraphType>
class GroupCfg< GraphType >