DART  6.6.2
dart::planning::RRT Class Reference

The rapidly-expanding random tree implementation. More...

#include <RRT.hpp>

Public Types

enum  StepResult { STEP_COLLISION , STEP_REACHED , STEP_PROGRESS }
 To get byte-aligned Eigen vectors. More...
 

Public Member Functions

 RRT (dart::simulation::WorldPtr world, dart::dynamics::SkeletonPtr robot, const std::vector< std::size_t > &dofs, const Eigen::VectorXd &root, double stepSize=0.02)
 
 RRT (simulation::WorldPtr world, dynamics::SkeletonPtr robot, const std::vector< std::size_t > &dofs, const std::vector< Eigen::VectorXd > &roots, double stepSize=0.02)
 Constructor with multiple roots (so, multiple trees) More...
 
virtual ~RRT ()
 Destructor. More...
 
bool connect ()
 Reach for a random node by repeatedly extending nodes from the nearest neighbor in the tree. More...
 
bool connect (const Eigen::VectorXd &target)
 Reach for a target by repeatedly extending nodes from the nearest neighbor. Stop if collide. More...
 
StepResult tryStep ()
 Try a single step with the given "stepSize" to a random configuration. Fail if collide. More...
 
StepResult tryStep (const Eigen::VectorXd &qtry)
 Try a single step with the given "stepSize" to the given configuration. Fail if collide. More...
 
virtual StepResult tryStepFromNode (const Eigen::VectorXd &qtry, int NNidx)
 Tries to extend tree towards provided sample. More...
 
virtual bool newConfig (std::list< Eigen::VectorXd > &intermediatePoints, Eigen::VectorXd &qnew, const Eigen::VectorXd &qnear, const Eigen::VectorXd &qtarget)
 Checks if the given new configuration is in collision with an obstacle. More...
 
double getGap (const Eigen::VectorXd &target)
 Returns the distance between the current active node and the given node. More...
 
void tracePath (int node, std::list< Eigen::VectorXd > &path, bool reverse=false)
 Traces the path from some node to the initConfig node - useful in creating the full path after the goal is reached. More...
 
std::size_t getSize ()
 Returns the number of nodes in the tree. More...
 
virtual bool checkCollisions (const Eigen::VectorXd &c)
 Implementation-specific function for checking collisions. More...
 
virtual Eigen::VectorXd getRandomConfig ()
 Returns a random configuration with the specified node IDs. More...
 

Public Attributes

const int ndim
 Number of dof we can manipulate (may be less than robot's) More...
 
const double stepSize
 Step size at each node creation. More...
 
int activeNode
 Last added node or the nearest node found after a search. More...
 
std::vector< int > parentVector
 The ith node in configVector has parent with index pV[i]. More...
 
std::vector< const Eigen::VectorXd * > configVector
 All visited configs. More...
 

Protected Member Functions

double randomInRange (double min, double max)
 Returns a random value between the given minimum and maximum value. More...
 
virtual int getNearestNeighbor (const Eigen::VectorXd &qsamp)
 Returns the nearest neighbor to query point. More...
 
virtual int addNode (const Eigen::VectorXd &qnew, int parentId)
 Adds a new node to the tree. More...
 

Protected Attributes

simulation::WorldPtr world
 The world that the robot is in. More...
 
dynamics::SkeletonPtr robot
 The ID of the robot for which a plan is generated. More...
 
std::vector< std::size_t > dofs
 The dofs of the robot the planner can manipulate. More...
 
flann::Index< flann::L2< double > > * index
 The underlying flann data structure for fast nearest neighbor searches. More...
 

Detailed Description

The rapidly-expanding random tree implementation.

Member Enumeration Documentation

◆ StepResult

To get byte-aligned Eigen vectors.

The result of attempting to create a new node to reach a target node

Enumerator
STEP_COLLISION 
STEP_REACHED 
STEP_PROGRESS 

Constructor & Destructor Documentation

◆ RRT() [1/2]

dart::planning::RRT::RRT ( dart::simulation::WorldPtr  world,
dart::dynamics::SkeletonPtr  robot,
const std::vector< std::size_t > &  dofs,
const Eigen::VectorXd &  root,
double  stepSize = 0.02 
)

◆ RRT() [2/2]

dart::planning::RRT::RRT ( simulation::WorldPtr  world,
dynamics::SkeletonPtr  robot,
const std::vector< std::size_t > &  dofs,
const std::vector< Eigen::VectorXd > &  roots,
double  stepSize = 0.02 
)

Constructor with multiple roots (so, multiple trees)

◆ ~RRT()

virtual dart::planning::RRT::~RRT ( )
inlinevirtual

Destructor.

Member Function Documentation

◆ addNode()

int dart::planning::RRT::addNode ( const Eigen::VectorXd &  qnew,
int  parentId 
)
protectedvirtual

Adds a new node to the tree.

◆ checkCollisions()

bool dart::planning::RRT::checkCollisions ( const Eigen::VectorXd &  c)
virtual

Implementation-specific function for checking collisions.

◆ connect() [1/2]

bool dart::planning::RRT::connect ( )

Reach for a random node by repeatedly extending nodes from the nearest neighbor in the tree.

Stop if there is a collision.

◆ connect() [2/2]

bool dart::planning::RRT::connect ( const Eigen::VectorXd &  target)

Reach for a target by repeatedly extending nodes from the nearest neighbor. Stop if collide.

◆ getGap()

double dart::planning::RRT::getGap ( const Eigen::VectorXd &  target)

Returns the distance between the current active node and the given node.

TODO This might mislead the users to thinking returning the distance between the given target and the nearest neighbor.

◆ getNearestNeighbor()

int dart::planning::RRT::getNearestNeighbor ( const Eigen::VectorXd &  qsamp)
inlineprotectedvirtual

Returns the nearest neighbor to query point.

◆ getRandomConfig()

VectorXd dart::planning::RRT::getRandomConfig ( )
virtual

Returns a random configuration with the specified node IDs.

◆ getSize()

std::size_t dart::planning::RRT::getSize ( )

Returns the number of nodes in the tree.

◆ newConfig()

bool dart::planning::RRT::newConfig ( std::list< Eigen::VectorXd > &  intermediatePoints,
Eigen::VectorXd &  qnew,
const Eigen::VectorXd &  qnear,
const Eigen::VectorXd &  qtarget 
)
virtual

Checks if the given new configuration is in collision with an obstacle.

Moreover, it is a an opportunity for child classes to change the new configuration if there is a need. For instance, task constrained planners might want to sample around this point and replace it with a better (less erroroneous due to constraint) node.

◆ randomInRange()

double dart::planning::RRT::randomInRange ( double  min,
double  max 
)
inlineprotected

Returns a random value between the given minimum and maximum value.

◆ tracePath()

void dart::planning::RRT::tracePath ( int  node,
std::list< Eigen::VectorXd > &  path,
bool  reverse = false 
)

Traces the path from some node to the initConfig node - useful in creating the full path after the goal is reached.

◆ tryStep() [1/2]

RRT::StepResult dart::planning::RRT::tryStep ( )

Try a single step with the given "stepSize" to a random configuration. Fail if collide.

◆ tryStep() [2/2]

StepResult dart::planning::RRT::tryStep ( const Eigen::VectorXd &  qtry)

Try a single step with the given "stepSize" to the given configuration. Fail if collide.

◆ tryStepFromNode()

RRT::StepResult dart::planning::RRT::tryStepFromNode ( const Eigen::VectorXd &  qtry,
int  NNidx 
)
virtual

Tries to extend tree towards provided sample.

Member Data Documentation

◆ activeNode

int dart::planning::RRT::activeNode

Last added node or the nearest node found after a search.

◆ configVector

std::vector<const Eigen::VectorXd*> dart::planning::RRT::configVector

All visited configs.

◆ dofs

std::vector<std::size_t> dart::planning::RRT::dofs
protected

The dofs of the robot the planner can manipulate.

◆ index

flann::Index<flann::L2<double> >* dart::planning::RRT::index
protected

The underlying flann data structure for fast nearest neighbor searches.

◆ ndim

const int dart::planning::RRT::ndim

Number of dof we can manipulate (may be less than robot's)

◆ parentVector

std::vector<int> dart::planning::RRT::parentVector

The ith node in configVector has parent with index pV[i].

◆ robot

dynamics::SkeletonPtr dart::planning::RRT::robot
protected

The ID of the robot for which a plan is generated.

◆ stepSize

const double dart::planning::RRT::stepSize

Step size at each node creation.

◆ world

simulation::WorldPtr dart::planning::RRT::world
protected

The world that the robot is in.