Simox  2.3.49
Saba::CSpace Class Referenceabstract

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

Inheritance diagram for Saba::CSpace:
Saba::CSpaceSampled

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 ()
 
unsigned int getRandomSeed () const
 get random seed More...
 
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)
 interpolates rotational between a and b using step as step size More...
 

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< CSpaceNodePtrnodes
 
std::vector< CSpaceNodePtrfreeNodes
 vector with pointers to really used nodes More...
 
std::vector< VirtualRobot::RobotNodePtrrobotJoints
 vector with pointers to free (not used) nodes More...
 
unsigned int randomSeed
 
float randMult
 
bool useMetricWeights
 
bool checkForBorderlessDims
 
std::vector< bool > borderLessDimension
 
bool multiThreaded
 
std::vector< ConfigurationConstraintPtrconstraints
 
SamplerPtr sampleAlgorithm
 

Static Protected Attributes

static int cloneCounter = 0
 
static boost::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

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.

Saba::CSpace::~CSpace ( )
virtual

destructor

Member Function Documentation

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

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
float Saba::CSpace::calcDist2 ( const Eigen::VectorXf &  c1,
const Eigen::VectorXf &  c2,
bool  forceDisablingMetricWeights = false 
)
float Saba::CSpace::calculateObstacleDistance ( const Eigen::VectorXf &  config)
virtual

calculate distance to obstacles

Parameters
configThe config for which the obstacle distance should be calculated (according to the cdm of this cspace)
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
enableWhen 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.
bool Saba::CSpace::checkSolution ( CSpacePathPtr  path,
bool  verbose = false 
)
virtual

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

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

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

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.

CSpaceNodePtr Saba::CSpace::createNewNode ( )
virtual
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.

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
startThe start configuration.
goalThe goal configuration.
storeAddedLengthThe 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.

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
enableWhen 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).
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
bGrantedWhen 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.
void Saba::CSpace::generateNewConfig ( const Eigen::VectorXf &  randomConfig,
const Eigen::VectorXf &  nearestConfig,
Eigen::VectorXf &  storeNewConfig,
float  stepSize,
float  preCalculatedDist = -1.0 
)
protectedvirtual
float Saba::CSpace::getBoundaryDist ( unsigned int  d)

get boundary distance of an index

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

get maximum boundary of an index

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

get minimum boundary of an index

VirtualRobot::CDManagerPtr Saba::CSpace::getCDManager ( ) const
unsigned int Saba::CSpace::getDimension ( ) const

get cspace dimension

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

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

Returns array of size dimension.

CSpaceNodePtr Saba::CSpace::getNode ( unsigned int  id)
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
storeValuesStore the values here.
checkValidWhen set to true, it is guaranteed that the sample not in collision and all constraints are considered
float Saba::CSpace::getRandomConfig_UniformSampling ( unsigned int  dim)
virtual

Returns a uniformly sampled random value for dimension dim.

unsigned int Saba::CSpace::getRandomSeed ( ) const
inline

get random seed

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

get the robot of cspace

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

get the sets of robotnodes representing of cspace

bool Saba::CSpace::hasExclusiveRobotAccess ( )
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 joint boundaries of a rotational joint are >= 2PI (and checkForBorderlessDimensions was not disabled), the correct direction for interpolating is determined automatically.

Eigen::VectorXf Saba::CSpace::interpolate ( const Eigen::VectorXf &  q1,
const Eigen::VectorXf &  q2,
float  step 
)
float Saba::CSpace::interpolateLinear ( float  a,
float  b,
float  step 
)
protected

interpolates linear between a and b using step as step size

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

interpolates rotational between a and b using step as step size

bool Saba::CSpace::isBorderlessDimension ( unsigned int  dim) const
bool Saba::CSpace::isCollisionFree ( const Eigen::VectorXf &  config)
virtual

returns the collision status (true for a valid config)

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)

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

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

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
q1first configuration (from)
q2second configuration (to)
Returns
true if path is valid

Reimplemented in Saba::CSpaceSampled.

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

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

void Saba::CSpace::lock ( )
static

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

void Saba::CSpace::printConfig ( const Eigen::VectorXf &  c) const
void Saba::CSpace::removeNode ( CSpaceNodePtr  node)
virtual
void Saba::CSpace::requestStop ( )

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

void Saba::CSpace::reset ( )
virtual
void Saba::CSpace::resetPerformanceVars ( )
inline
void Saba::CSpace::respectBoundaries ( Eigen::VectorXf &  config)
virtual

set values of configuration considering boundaries

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
minminimum values (array has to have same dimension as the cspace)
maxmaximum values (array has to have same dimension as the cspace)
void Saba::CSpace::setBoundary ( unsigned int  dim,
float  min,
float  max 
)
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.

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.
void Saba::CSpace::setRandomSeed ( unsigned int  random_seed)
inline
void Saba::CSpace::unlock ( )
static

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

Field Documentation

std::vector< bool > Saba::CSpace::borderLessDimension
protected
Eigen::VectorXf Saba::CSpace::boundaryDist
protected

boundaries of this c-space

Eigen::VectorXf Saba::CSpace::boundaryMax
protected
Eigen::VectorXf Saba::CSpace::boundaryMin
protected
VirtualRobot::CDManagerPtr Saba::CSpace::cdm
protected

handling of collision detections

bool Saba::CSpace::checkForBorderlessDims
protected
SABA_IMPORT_EXPORT int Saba::CSpace::cloneCounter = 0
staticprotected
SABA_IMPORT_EXPORT boost::mutex Saba::CSpace::colCheckMutex
staticprotected
std::vector<ConfigurationConstraintPtr> Saba::CSpace::constraints
protected
unsigned int Saba::CSpace::dimension
protected

dimension of this c-space

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

vector with pointers to really used nodes

int Saba::CSpace::maxNodes
protected
Eigen::VectorXf Saba::CSpace::metricWeights
protected

weights for distance computation

bool Saba::CSpace::multiThreaded
protected
std::vector< CSpaceNodePtr > Saba::CSpace::nodes
protected
int Saba::CSpace::performaceVars_collisionCheck
int Saba::CSpace::performaceVars_distanceCheck
float Saba::CSpace::randMult
protected
unsigned int Saba::CSpace::randomSeed
protected
VirtualRobot::RobotPtr Saba::CSpace::robo
protected

the robot for collision checking

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

vector with pointers to free (not used) nodes

joints of the robot that we are manipulating

VirtualRobot::RobotNodeSetPtr Saba::CSpace::robotNodes
protected

the robot nodes defining the c-space

SamplerPtr Saba::CSpace::sampleAlgorithm
protected
bool Saba::CSpace::stopPathCheck
protected
bool Saba::CSpace::useMetricWeights
protected