This class represents a sampled-based configuration space. This is the main class for RRT-related planning. More...

Inheritance diagram for Saba::CSpaceSampled:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	CSpaceSampled (VirtualRobot::RobotPtr robot, VirtualRobot::CDManagerPtr collisionManager, VirtualRobot::RobotNodeSetPtr robotNodes, unsigned int maxConfigs=50000, unsigned int randomSeed=0)

	~CSpaceSampled () override

void	setSamplingSize (float fSize)
	sets sampling step size (used when adding new paths in CSpace) More...

void	setSamplingSizeDCD (float fSize)
	sets sampling step size used for discrete collision checking More...

float	getSamplingSize ()

float	getSamplingSizeDCD ()

CSpacePtr	clone (VirtualRobot::CollisionCheckerPtr newColChecker, VirtualRobot::RobotPtr newRobot, VirtualRobot::CDManagerPtr newCDM, unsigned int newRandomSeed=0) override

bool	isPathValid (const Eigen::VectorXf &q1, const Eigen::VectorXf &q2) override

CSpacePathPtr	createPath (const Eigen::VectorXf &start, const Eigen::VectorXf &goal) override

CSpacePathPtr	createPathUntilInvalid (const Eigen::VectorXf &start, const Eigen::VectorXf &goal, float &storeAddedLength) override

Public Member Functions inherited from Saba::CSpace
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	CSpace (VirtualRobot::RobotPtr robot, VirtualRobot::CDManagerPtr collisionManager, VirtualRobot::RobotNodeSetPtr robotNodes, unsigned int maxConfigs=50000, unsigned int randomSeed=0)

virtual	~CSpace ()
	destructor More...

void	setBoundaries (const Eigen::VectorXf &min, const Eigen::VectorXf &max)

void	setBoundary (unsigned int dim, float min, float max)

void	setMetricWeights (const Eigen::VectorXf &weights)

float	getBoundaryMin (unsigned int d)
	get minimum boundary of an index More...

float	getBoundaryMax (unsigned int d)
	get maximum boundary of an index More...

float	getBoundaryDist (unsigned int d)
	get boundary distance of an index More...

bool	isBorderlessDimension (unsigned int dim) const

unsigned int	getDimension () const
	get cspace dimension More...

VirtualRobot::RobotPtr	getRobot () const
	get the robot of cspace More...

VirtualRobot::RobotNodeSetPtr	getRobotNodeSet () const
	get the sets of robotnodes representing of cspace More...

VirtualRobot::CDManagerPtr	getCDManager () const

virtual float	getRandomConfig_UniformSampling (unsigned int dim)

void	getRandomConfig (Eigen::VectorXf &storeValues, bool checkValid=false)

virtual void	respectBoundaries (Eigen::VectorXf &config)
	set values of configuration considering boundaries More...

void	requestStop ()

void	setRandomSeed (unsigned int random_seed)

virtual bool	checkSolution (CSpacePathPtr path, bool verbose=false)
	checks complete solution path (with the pathCheck methods provided by the CSpace implementation) More...

virtual bool	checkTree (CSpaceTreePtr tree)
	checks complete tree (with the pathCheck methods provided by the CSpace implementation) More...

void	resetPerformanceVars ()

virtual void	reset ()

Eigen::VectorXf	getMetricWeights ()

void	enableWeights (bool enable)

void	exclusiveRobotAccess (bool bGranted=true)

bool	hasExclusiveRobotAccess ()

void	setRandomSampler (SamplerPtr sampler)

float	calcDist (const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, bool forceDisablingMetricWeights=false)

float	calcDist2 (const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, bool forceDisablingMetricWeights=false)

virtual float	calculateObstacleDistance (const Eigen::VectorXf &config)
	calculate distance to obstacles More...

CSpaceNodePtr	getNode (unsigned int id)

virtual CSpaceNodePtr	createNewNode ()

virtual void	removeNode (CSpaceNodePtr node)

virtual bool	isCollisionFree (const Eigen::VectorXf &config)
	returns the collision status (true for a valid config) More...

virtual bool	isInBoundary (const Eigen::VectorXf &config)
	returns the boundary violation status (true for a valid config) More...

virtual bool	isSatisfyingConstraints (const Eigen::VectorXf &config)
	returns the constraint violation status (true for a valid config) More...

void	printConfig (const Eigen::VectorXf &c) const

void	checkForBorderlessDimensions (bool enable)

float	interpolate (const Eigen::VectorXf &q1, const Eigen::VectorXf &q2, int dim, float step)

Eigen::VectorXf	interpolate (const Eigen::VectorXf &q1, const Eigen::VectorXf &q2, float step)

virtual bool	isConfigValid (const Eigen::VectorXf &pConfig, bool checkBorders=true, bool checkCollisions=true, bool checkConstraints=true)
	check whether a configuration is valid (collision, boundary, and constraints check) More...

virtual void	addConstraintCheck (Saba::ConfigurationConstraintPtr constraint)

Protected Attributes
float	samplingSizePaths
	euclidean sample size More...

float	samplingSizeDCD
	euclidean sample size for collision check More...

Eigen::VectorXf	checkPathConfig

int	recursiveTmpValuesIndex

std::vector< Eigen::VectorXf >	recursiveTmpValues

Eigen::VectorXf	tmpConfig

const int	recursionMaxDepth

Protected Attributes inherited from Saba::CSpace
unsigned int	dimension
	dimension of this c-space More...

Eigen::VectorXf	boundaryMax

Eigen::VectorXf	boundaryMin

Eigen::VectorXf	boundaryDist
	boundaries of this c-space More...

Eigen::VectorXf	metricWeights
	weights for distance computation More...

bool	stopPathCheck

VirtualRobot::RobotPtr	robo
	the robot for collision checking More...

VirtualRobot::RobotNodeSetPtr	robotNodes
	the robot nodes defining the c-space More...

VirtualRobot::CDManagerPtr	cdm
	handling of collision detections More...

int	maxNodes

std::vector< CSpaceNodePtr >	nodes

std::vector< CSpaceNodePtr >	freeNodes
	vector with pointers to really used nodes More...

std::vector< VirtualRobot::RobotNodePtr >	robotJoints
	vector with pointers to free (not used) nodes More...

bool	useMetricWeights

bool	checkForBorderlessDims

std::vector< bool >	borderLessDimension

bool	multiThreaded

std::vector< ConfigurationConstraintPtr >	constraints

SamplerPtr	sampleAlgorithm

Additional Inherited Members
Static Public Member Functions inherited from Saba::CSpace
static void	lock ()
	if multithreading is enabled, the colChecking mutex can be locked/unlocked externally More...

static void	unlock ()
	if multithreading is enabled, the colChecking mutex can be locked/unlocked externally More...

Data Fields inherited from Saba::CSpace
int	performaceVars_collisionCheck

int	performaceVars_distanceCheck

Protected Member Functions inherited from Saba::CSpace
virtual void	getDirectionVector (const Eigen::VectorXf &c1, const Eigen::VectorXf &c2, Eigen::VectorXf &storeDir, float length)

virtual void	generateNewConfig (const Eigen::VectorXf &randomConfig, const Eigen::VectorXf &nearestConfig, Eigen::VectorXf &storeNewConfig, float stepSize, float preCalculatedDist=-1.0)

virtual float	getDirectedMaxMovement (const Eigen::VectorXf &config, const Eigen::VectorXf &nextConfig)
	return upper limit for movement of any point on joints if moving from config to nextConfig More...

float	interpolateLinear (float a, float b, float step)
	interpolates linear between a and b using step as step size More...

float	interpolateRotational (float a, float b, float step)

Static Protected Attributes inherited from Saba::CSpace
static int	cloneCounter = 0

static std::mutex	colCheckMutex

Detailed Description

This class represents a sampled-based configuration space. This is the main class for RRT-related planning.

A CSpace is defined by a set of robot nodes and a collision manager. The RobotNodeSet specifies the dimension and the borders of the CSpace. The collision manager is used to check configurations for collisions. Here, multiple sets of objects can be defined which are internally mutually checked for collisions. This allows to specify complex collision queries. The sampling-based c-space relies on two parameters: The sampling size, which specifies the (c-space) distance between two succeeding configurations on a path segment. This parameter affects how many intermediate configurations are added when creating new paths. The second parameter is called DCD sampling size, where DCD stands for discrete collision detection. This parameter is needed for the check if a path segment is valid or not. Therefore intermediate configurations on the path segment are checked for collisions with the given maximum distance between two neighboring intermediate configuration. This parameter affects both, the performance (the Rrt approach spends most of the time for collision detection) and the reliability of the results. Since there is no guarantee that all potential configurations are detected with sampling-based approaches, a large DCD sampling parameter will increase the chance of missing a collision.

Constraints can be considered by adding instances of ConfigurationConstraintPtr.

See also: CSpace; Rrt; BiRrt; VirtualRobot::CDManager

Constructor & Destructor Documentation

◆ CSpaceSampled()

Saba::CSpaceSampled::CSpaceSampled	(	VirtualRobot::RobotPtr	robot,
		VirtualRobot::CDManagerPtr	collisionManager,
		VirtualRobot::RobotNodeSetPtr	robotNodes,
		unsigned int	maxConfigs = `50000`,
		unsigned int	randomSeed = `0`
	)

Construct a c-space that represents the given set of joints. The dimension of this c-space is related to the number of joints in robotNodes. The boundaries of this c-space are set according to definitions in robotNodes.

◆ ~CSpaceSampled()

Saba::CSpaceSampled::~CSpaceSampled ( )

overridedefault

Member Function Documentation

◆ clone()

CSpacePtr Saba::CSpaceSampled::clone	(	VirtualRobot::CollisionCheckerPtr	newCollisionChecker,
		VirtualRobot::RobotPtr	newRobot,
		VirtualRobot::CDManagerPtr	newCDM,
		unsigned int	nNewRandomSeed = `0`
	)

overridevirtual

Clone this CSpace structure The new Robot and the new CCM are used, the robot and the ccm have to be linked to the new ColChecker!

Implements Saba::CSpace.

◆ createPath()

CSpacePathPtr Saba::CSpaceSampled::createPath	(	const Eigen::VectorXf &	start,
		const Eigen::VectorXf &	goal
	)

overridevirtual

Create a path from start to goal without any checks. Intermediate configurations are added according to the current implementation of the cspace.

Reimplemented from Saba::CSpace.

◆ createPathUntilInvalid()

Saba::CSpacePathPtr Saba::CSpaceSampled::createPathUntilInvalid	(	const Eigen::VectorXf &	start,
		const Eigen::VectorXf &	goal,
		float &	storeAddedLength
	)

overridevirtual

Create a path from start to the goal configuration. In case an invalid (collision/constraints) position is detected the appending is stopped and the valid part of the path is returned.

Parameters

start	The start
goal	The goal
storeAddedLength	The length of the valid path is stored here (1.0 means the complete path from start to goal was valid)

Reimplemented from Saba::CSpace.

◆ getSamplingSize()

float Saba::CSpaceSampled::getSamplingSize ( )

inline

◆ getSamplingSizeDCD()

float Saba::CSpaceSampled::getSamplingSizeDCD ( )

inline

◆ isPathValid()

bool Saba::CSpaceSampled::isPathValid	(	const Eigen::VectorXf &	q1,
		const Eigen::VectorXf &	q2
	)

overridevirtual

Checks the middle configuration and when it is not invalid (in collision or constraints are violated) the path is split and both parts are recursively checked until the distance is smalled than the DCD sampling size. Recursion is performed maximal recursionMaxDepth times, since an array of temporary variables is used, in order to avoid slow allocating/deallocating of memory.

Reimplemented from Saba::CSpace.

◆ setSamplingSize()

void Saba::CSpaceSampled::setSamplingSize ( float fSize )

sets sampling step size (used when adding new paths in CSpace)

◆ setSamplingSizeDCD()

void Saba::CSpaceSampled::setSamplingSizeDCD ( float fSize )

sets sampling step size used for discrete collision checking

Field Documentation

◆ checkPathConfig

Eigen::VectorXf Saba::CSpaceSampled::checkPathConfig

protected

◆ recursionMaxDepth

const int Saba::CSpaceSampled::recursionMaxDepth

protected

◆ recursiveTmpValues

std::vector<Eigen::VectorXf> Saba::CSpaceSampled::recursiveTmpValues

protected

◆ recursiveTmpValuesIndex

int Saba::CSpaceSampled::recursiveTmpValuesIndex

protected

◆ samplingSizeDCD

float Saba::CSpaceSampled::samplingSizeDCD

protected

euclidean sample size for collision check

◆ samplingSizePaths

float Saba::CSpaceSampled::samplingSizePaths

protected

euclidean sample size

◆ tmpConfig

Eigen::VectorXf Saba::CSpaceSampled::tmpConfig

protected

Public Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ CSpaceSampled()

◆ ~CSpaceSampled()

Member Function Documentation

◆ clone()

◆ createPath()

◆ createPathUntilInvalid()

◆ getSamplingSize()

◆ getSamplingSizeDCD()

◆ isPathValid()

◆ setSamplingSize()

◆ setSamplingSizeDCD()

Field Documentation

◆ checkPathConfig

◆ recursionMaxDepth

◆ recursiveTmpValues

◆ recursiveTmpValuesIndex

◆ samplingSizeDCD

◆ samplingSizePaths

◆ tmpConfig