A c-space interface for motion planning. More...

Inheritance diagram for Saba::CSpace:

Public Member Functions
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...

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

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

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...

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

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

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

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)

Static Public Member Functions
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
int	performaceVars_collisionCheck

int	performaceVars_distanceCheck

Protected Member Functions
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)

Protected Attributes
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

Static Protected Attributes
static int	cloneCounter = 0

static std::mutex	colCheckMutex

Detailed Description

A c-space interface for motion 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. When performing sampling-based motion planning, usually paths in c-space have to be verified if constraints are violated or collisions occur. Therefore this class provides an interface, in order to combine various planning approaches with different collision detection methods

Please note: This implementation is not able to determine the collision status of path segments. Use CSpaceSampled for discrete collision detection.

See also: VirtualRobot::CDManager; MotionPlanner, Rrt, BiRrt; CSpaceSampled

Constructor & Destructor Documentation

◆ CSpace()

Saba::CSpace::CSpace	(	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 dimensionality of this c-space is set by the number of nodes in robotNodes. The boundaries of this c-space are set according to definitions in robotNodes. On startup, memory is allocate din order to allow fats processing on runtime.

◆ ~CSpace()

Saba::CSpace::~CSpace ( )

virtual

destructor

Member Function Documentation

◆ addConstraintCheck()

void Saba::CSpace::addConstraintCheck ( Saba::ConfigurationConstraintPtr constraint )

virtual

Add a configuration constraint to be checked within this cspace. Standard: No constraints, meaning that a check for constraints will report a valid status

◆ calcDist()

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

Compute the distance in c-space. The modes useMetricWeights (standard:disabled) and checkForBorderlessDims (stanbdard:enabled) may affect the results.

See also: setMetricWeights; checkForBorderlessDimensions

◆ calcDist2()

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

◆ calculateObstacleDistance()

float Saba::CSpace::calculateObstacleDistance ( const Eigen::VectorXf & config )

virtual

calculate distance to obstacles

Parameters

config The config for which the obstacle distance should be calculated (according to the cdm of this cspace)

◆ checkForBorderlessDimensions()

void Saba::CSpace::checkForBorderlessDimensions ( bool enable )

When the size of a c-space dimension is > 2PI and the corresponding joint is rotational, it is assumed that there are no borders

Parameters

enable When set, for each c-space dimension the (rotational) corresponding joints are checked: Borderless dimension, if the distance between Lo and Hi limit is >2PI This is useful e.g. for free flying or holonomic moving robots. Translational joints are not considered.

◆ checkSolution()

bool Saba::CSpace::checkSolution	(	CSpacePathPtr	path,
		bool	verbose = `false`
	)

virtual

checks complete solution path (with the pathCheck methods provided by the CSpace implementation)

◆ checkTree()

bool Saba::CSpace::checkTree ( CSpaceTreePtr tree )

virtual

checks complete tree (with the pathCheck methods provided by the CSpace implementation)

◆ clone()

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

pure virtual

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!

Implemented in Saba::CSpaceSampled.

◆ createNewNode()

CSpaceNodePtr Saba::CSpace::createNewNode ( )

virtual

◆ createPath()

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

virtual

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

Reimplemented in Saba::CSpaceSampled.

◆ createPathUntilInvalid()

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

virtual

Create a path from start to the goal configuration until an invalid (collision,constraints) configuration is found. Intermediate configurations are added according to the current implementation of the cspace In this implementation only the start and goal config are added to the path.

Parameters

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

Reimplemented in Saba::CSpaceSampled.

◆ enableWeights()

void Saba::CSpace::enableWeights ( bool enable )

Enable/Disable weighting of c-space dimensions. (standard: disabled) By setting the weights with setMetricWeights() the weighting is enabled.

Parameters

enable When set, the metric weights of this c-space are used to compute distances (when no metric weights have been specified for this c-space the option has no effect).

◆ exclusiveRobotAccess()

void Saba::CSpace::exclusiveRobotAccess ( bool bGranted = true )

Method to allow concurrent/parallel access on robot. Enabling the multiThreading support is only needed when sharing robot or environment models between multiple CSpaces and the collision detection is done in parallel. If each CSpace operates on it's own instances of robot and environment, the multithreading support must not be enabled. Also rrtBiPlanners do not need to enable these option. If enabled, the robot is locked for collision checking, so that only one instance operates on the model. Planners operate much faster when doing "real" parallel collision checking, meaning that multiple instances of the models are created and these instances have their own collision checker.

Parameters

bGranted When set to true, no mutex protection is used to access the robot and to perform the collision detection. (standard) If set to false, the Robot and CD calls are protected by a mutex.

◆ generateNewConfig()

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

protectedvirtual

◆ getBoundaryDist()

float Saba::CSpace::getBoundaryDist ( unsigned int d )

get boundary distance of an index

◆ getBoundaryMax()

float Saba::CSpace::getBoundaryMax ( unsigned int d )

get maximum boundary of an index

◆ getBoundaryMin()

float Saba::CSpace::getBoundaryMin ( unsigned int d )

get minimum boundary of an index

◆ getCDManager()

VirtualRobot::CDManagerPtr Saba::CSpace::getCDManager ( ) const

◆ getDimension()

unsigned int Saba::CSpace::getDimension ( ) const

get cspace dimension

◆ getDirectedMaxMovement()

float Saba::CSpace::getDirectedMaxMovement	(	const Eigen::VectorXf &	config,
		const Eigen::VectorXf &	nextConfig
	)

protectedvirtual

return upper limit for movement of any point on joints if moving from config to nextConfig

◆ getDirectionVector()

void Saba::CSpace::getDirectionVector	(	const Eigen::VectorXf &	c1,
		const Eigen::VectorXf &	c2,
		Eigen::VectorXf &	storeDir,
		float	length
	)

protectedvirtual

◆ getMetricWeights()

Eigen::VectorXf Saba::CSpace::getMetricWeights ( )

inline

Returns array of size dimension.

◆ getNode()

CSpaceNodePtr Saba::CSpace::getNode ( unsigned int id )

◆ getRandomConfig()

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

This method returns a random configuration. The value is generated uniformly (standard) or, in case a sampler is defined a custom sampling strategy can be used.

Parameters

storeValues	Store the values here.
checkValid	When set to true, it is guaranteed that the sample not in collision and all constraints are considered

◆ getRandomConfig_UniformSampling()

float Saba::CSpace::getRandomConfig_UniformSampling ( unsigned int dim )

virtual

Returns a uniformly sampled random value for dimension dim.

◆ getRobot()

VirtualRobot::RobotPtr Saba::CSpace::getRobot ( ) const

get the robot of cspace

◆ getRobotNodeSet()

VirtualRobot::RobotNodeSetPtr Saba::CSpace::getRobotNodeSet ( ) const

get the sets of robotnodes representing of cspace

◆ hasExclusiveRobotAccess()

bool Saba::CSpace::hasExclusiveRobotAccess ( )

◆ interpolate() [1/2]

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

Interpolates between two values in dimension dim. Checks weather the corresponding joint moves translational or a rotational and performs the interpolation accordingly. When a rotational joint is limitless (and checkForBorderlessDimensions was not disabled), the correct direction for interpolating is determined automatically.

◆ interpolate() [2/2]

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

◆ interpolateLinear()

float Saba::CSpace::interpolateLinear	(	float	a,
		float	b,
		float	step
	)

protected

interpolates linear between a and b using step as step size

◆ interpolateRotational()

float Saba::CSpace::interpolateRotational	(	float	a,
		float	b,
		float	step
	)

protected

interpolates rotational between a and b using step as step size ('a' and 'b' are expected to be in [0,2pi]). The interpolation takes the direction of the shorter path between 'a' and 'b'.

◆ isBorderlessDimension()

bool Saba::CSpace::isBorderlessDimension ( unsigned int dim ) const

◆ isCollisionFree()

bool Saba::CSpace::isCollisionFree ( const Eigen::VectorXf & config )

virtual

returns the collision status (true for a valid config)

◆ isConfigValid()

bool Saba::CSpace::isConfigValid	(	const Eigen::VectorXf &	pConfig,
		bool	checkBorders = `true`,
		bool	checkCollisions = `true`,
		bool	checkConstraints = `true`
	)

virtual

check whether a configuration is valid (collision, boundary, and constraints check)

◆ isInBoundary()

bool Saba::CSpace::isInBoundary ( const Eigen::VectorXf & config )

virtual

returns the boundary violation status (true for a valid config)

◆ isPathValid()

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

virtual

Check path between two configurations. This cspace implementation just checks q2!

Parameters

q1	first configuration (from)
q2	second configuration (to)

Returns: true if path is valid

Reimplemented in Saba::CSpaceSampled.

◆ isSatisfyingConstraints()

bool Saba::CSpace::isSatisfyingConstraints ( const Eigen::VectorXf & config )

virtual

returns the constraint violation status (true for a valid config)

◆ lock()

void Saba::CSpace::lock ( )

static

if multithreading is enabled, the colChecking mutex can be locked/unlocked externally

◆ printConfig()

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

◆ removeNode()

void Saba::CSpace::removeNode ( CSpaceNodePtr node )

virtual

◆ requestStop()

void Saba::CSpace::requestStop ( )

Method for externally stopping the path checking routines. Only useful in multi-threaded planners.

◆ reset()

void Saba::CSpace::reset ( )

virtual

◆ resetPerformanceVars()

void Saba::CSpace::resetPerformanceVars ( )

inline

◆ respectBoundaries()

void Saba::CSpace::respectBoundaries ( Eigen::VectorXf & config )

virtual

set values of configuration considering boundaries

◆ setBoundaries()

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

Set boundary values for each dimension of the cspace. Only needed when other values needed than those defined in the corresponding robotNodes.

Parameters

min	minimum values (array has to have same dimension as the cspace)
max	maximum values (array has to have same dimension as the cspace)

◆ setBoundary()

void Saba::CSpace::setBoundary	(	unsigned int	dim,
		float	min,
		float	max
	)

◆ setMetricWeights()

void Saba::CSpace::setMetricWeights ( const Eigen::VectorXf & weights )

Set weights to uniformly handle differing dimensions in c-space. Setting the weights with this methods enables weighting of distance calculations in c-space. This affects path-creation and distance calculations.

◆ setRandomSampler()

void Saba::CSpace::setRandomSampler ( SamplerPtr sampler )

In the standard setup no sampler is used and the method getRandomConfig() performs a uniformly sampling of random configurations. Since most planners use the getRandomConfig() method for generating their random configurations (

See also: Rrt), this method offers the possibility to exchange the sampling routine by a custom sample algorithm (e.g. a brideover-sampler or a Gaussian sampler can be realized). In case NULL is passed, the sampling routine is set back to the standard uniform sample algorithm.

◆ setRandomSeed()

void Saba::CSpace::setRandomSeed ( unsigned int random_seed )

◆ unlock()

void Saba::CSpace::unlock ( )

static

if multithreading is enabled, the colChecking mutex can be locked/unlocked externally

Field Documentation

◆ borderLessDimension

std::vector< bool > Saba::CSpace::borderLessDimension

protected

◆ boundaryDist

Eigen::VectorXf Saba::CSpace::boundaryDist

protected

boundaries of this c-space

◆ boundaryMax

Eigen::VectorXf Saba::CSpace::boundaryMax

protected

◆ boundaryMin

Eigen::VectorXf Saba::CSpace::boundaryMin

protected

◆ cdm

VirtualRobot::CDManagerPtr Saba::CSpace::cdm

protected

handling of collision detections

◆ checkForBorderlessDims

bool Saba::CSpace::checkForBorderlessDims

protected

◆ cloneCounter

SABA_IMPORT_EXPORT int Saba::CSpace::cloneCounter = 0

staticprotected

◆ colCheckMutex

SABA_IMPORT_EXPORT std::mutex Saba::CSpace::colCheckMutex

staticprotected

◆ constraints

std::vector<ConfigurationConstraintPtr> Saba::CSpace::constraints

protected

◆ dimension

unsigned int Saba::CSpace::dimension

protected

dimension of this c-space

◆ freeNodes

std::vector< CSpaceNodePtr > Saba::CSpace::freeNodes

protected

vector with pointers to really used nodes

◆ maxNodes

int Saba::CSpace::maxNodes

protected

◆ metricWeights

Eigen::VectorXf Saba::CSpace::metricWeights

protected

weights for distance computation

◆ multiThreaded

bool Saba::CSpace::multiThreaded

protected

◆ nodes

std::vector< CSpaceNodePtr > Saba::CSpace::nodes

protected

◆ performaceVars_collisionCheck

int Saba::CSpace::performaceVars_collisionCheck

◆ performaceVars_distanceCheck

int Saba::CSpace::performaceVars_distanceCheck

◆ robo

VirtualRobot::RobotPtr Saba::CSpace::robo

protected

the robot for collision checking

◆ robotJoints

std::vector<VirtualRobot::RobotNodePtr> Saba::CSpace::robotJoints

protected

vector with pointers to free (not used) nodes

joints of the robot that we are manipulating

◆ robotNodes

VirtualRobot::RobotNodeSetPtr Saba::CSpace::robotNodes

protected

the robot nodes defining the c-space

◆ sampleAlgorithm

SamplerPtr Saba::CSpace::sampleAlgorithm

protected

◆ stopPathCheck

bool Saba::CSpace::stopPathCheck

protected

◆ useMetricWeights

bool Saba::CSpace::useMetricWeights

protected

Public Member Functions

Static Public Member Functions

Data Fields

Protected Member Functions

Protected Attributes

Static Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ CSpace()

◆ ~CSpace()

Member Function Documentation

◆ addConstraintCheck()

◆ calcDist()

◆ calcDist2()

◆ calculateObstacleDistance()

◆ checkForBorderlessDimensions()

◆ checkSolution()

◆ checkTree()

◆ clone()

◆ createNewNode()

◆ createPath()

◆ createPathUntilInvalid()

◆ enableWeights()

◆ exclusiveRobotAccess()

◆ generateNewConfig()

◆ getBoundaryDist()

◆ getBoundaryMax()

◆ getBoundaryMin()

◆ getCDManager()

◆ getDimension()

◆ getDirectedMaxMovement()

◆ getDirectionVector()

◆ getMetricWeights()

◆ getNode()

◆ getRandomConfig()

◆ getRandomConfig_UniformSampling()

◆ getRobot()

◆ getRobotNodeSet()

◆ hasExclusiveRobotAccess()

◆ interpolate() [1/2]

◆ interpolate() [2/2]

◆ interpolateLinear()

◆ interpolateRotational()

◆ isBorderlessDimension()

◆ isCollisionFree()

◆ isConfigValid()

◆ isInBoundary()

◆ isPathValid()

◆ isSatisfyingConstraints()

◆ lock()

◆ printConfig()

◆ removeNode()

◆ requestStop()

◆ reset()

◆ resetPerformanceVars()

◆ respectBoundaries()

◆ setBoundaries()

◆ setBoundary()

◆ setMetricWeights()

◆ setRandomSampler()

◆ setRandomSeed()

◆ unlock()

Field Documentation

◆ borderLessDimension

◆ boundaryDist

◆ boundaryMax

◆ boundaryMin

◆ cdm

◆ checkForBorderlessDims

◆ cloneCounter

◆ colCheckMutex

◆ constraints

◆ dimension

◆ freeNodes

◆ maxNodes

◆ metricWeights

◆ multiThreaded

◆ nodes

◆ performaceVars_collisionCheck

◆ performaceVars_distanceCheck