Simox  2.3.74.0
Saba::CSpaceSampled Class Reference

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

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

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< 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...
 
bool useMetricWeights
 
bool checkForBorderlessDims
 
std::vector< bool > borderLessDimension
 
bool multiThreaded
 
std::vector< ConfigurationConstraintPtrconstraints
 
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
startThe start
goalThe goal
storeAddedLengthThe 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