DART
6.10.1
|
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. More... | |
StepResult | tryStep () |
Try a single step with the given "stepSize" to a random configuration. More... | |
StepResult | tryStep (const Eigen::VectorXd &qtry) |
Try a single step with the given "stepSize" to the given configuration. 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... | |
The rapidly-expanding random tree implementation.
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 |
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 |
||
) |
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)
|
inlinevirtual |
Destructor.
|
protectedvirtual |
Adds a new node to the tree.
|
virtual |
Implementation-specific function for checking collisions.
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.
bool dart::planning::RRT::connect | ( | const Eigen::VectorXd & | target | ) |
Reach for a target by repeatedly extending nodes from the nearest neighbor.
Stop if collide.
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.
|
inlineprotectedvirtual |
Returns the nearest neighbor to query point.
|
virtual |
Returns a random configuration with the specified node IDs.
std::size_t dart::planning::RRT::getSize | ( | ) |
Returns the number of nodes in the tree.
|
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.
|
inlineprotected |
Returns a random value between the given minimum and maximum value.
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.
RRT::StepResult dart::planning::RRT::tryStep | ( | ) |
Try a single step with the given "stepSize" to a random configuration.
Fail if collide.
StepResult dart::planning::RRT::tryStep | ( | const Eigen::VectorXd & | qtry | ) |
Try a single step with the given "stepSize" to the given configuration.
Fail if collide.
|
virtual |
Tries to extend tree towards provided sample.
int dart::planning::RRT::activeNode |
Last added node or the nearest node found after a search.
std::vector<const Eigen::VectorXd*> dart::planning::RRT::configVector |
All visited configs.
|
protected |
The dofs of the robot the planner can manipulate.
|
protected |
The underlying flann data structure for fast nearest neighbor searches.
const int dart::planning::RRT::ndim |
Number of dof we can manipulate (may be less than robot's)
std::vector<int> dart::planning::RRT::parentVector |
The ith node in configVector has parent with index pV[i].
|
protected |
The ID of the robot for which a plan is generated.
const double dart::planning::RRT::stepSize |
Step size at each node creation.
|
protected |
The world that the robot is in.