Optimal Rapidly-exploring Random Trees. More...
#include <RRTstar.h>
Classes | |
class | Motion |
Representation of a motion. More... | |
Public Member Functions | |
virtual void | clear (void) |
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() will ignore all previous work. | |
double | getBallRadiusConstant (void) const |
Get the multiplicative factor used in the computation of the radius whithin which tree rewiring is done. | |
bool | getDelayCC (void) const |
Get the state of the delayed collision checking option. | |
double | getGoalBias (void) const |
Get the goal bias the planner is using. | |
double | getMaxBallRadius (void) const |
Get the maximum radius the planner uses in the tree rewiring step. | |
virtual void | getPlannerData (base::PlannerData &data) const |
Get information about the current run of the motion planner. Repeated calls to this function will update data (only additions are made). This is useful to see what changed in the exploration datastructure, between calls to solve(), for example (without calling clear() in between). | |
double | getRange (void) const |
Get the range the planner is using. | |
bool | getTerminate (void) const |
Get the state of the termination option. | |
RRTstar (const base::SpaceInformationPtr &si) | |
void | setBallRadiusConstant (double ballRadiusConstant) |
When the planner attempts to rewire the tree, it does so by looking at some of the neighbors within a computed radius. The computation of that radius depends on the multiplicative factor set here. Set this parameter should be set at least to the side length of the (bounded) state space. E.g., if the state space is a box with side length L, then this parameter should be set to at least L for rapid and efficient convergence in trajectory space. | |
void | setDelayCC (bool delayCC) |
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cost. The planner then goes through this list, starting with the lowest cost, checking for collisions in order to find a parent. The planner stops iterating through the list when a collision free parent is found. This prevents the planner from collsion checking each neighbor, reducing computation time in scenarios where collision checking procedures are expensive. | |
void | setGoalBias (double goalBias) |
Set the goal bias. | |
void | setMaxBallRadius (double maxBallRadius) |
When the planner attempts to rewire the tree, it does so by looking at some of the neighbors within a computed radius. That radius is bounded by the value set here. This parameter should ideally be equal longest straight line from the initial state to anywhere in the state space. In other words, this parameter should be "sqrt(d) L", where d is the dimensionality of space and L is the side length of a box containing the obstacle free space. | |
template<template< typename T > class NN> | |
void | setNearestNeighbors (void) |
Set a different nearest neighbors datastructure. | |
void | setRange (double distance) |
Set the range the planner is supposed to use. | |
void | setTerminate (bool terminate) |
Option that specifies if the planner will terminate upon finding a solution or will continue to refine the solution until the time limit is reached. Set true to terminate when a solution is found. | |
virtual void | setup (void) |
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceInformation::setup() if needed. This must be called before solving. | |
virtual bool | solve (const base::PlannerTerminationCondition &ptc) |
Function that can solve the motion planning problem. This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work more time on an unsolved problem, for example. If this option is used, it is assumed the problem definition is not changed (unpredictable results otherwise). The only change in the problem definition that is accounted for is the addition of starting or goal states (but not changing previously added start/goal states). The function terminates if the call to ptc returns true. | |
virtual | ~RRTstar (void) |
Protected Member Functions | |
double | distanceFunction (const Motion *a, const Motion *b) const |
Compute distance between motions (actually distance between contained states). | |
void | freeMemory (void) |
Free the memory allocated by this planner. | |
Static Protected Member Functions | |
static bool | compareMotion (const Motion *a, const Motion *b) |
Sort the near neighbors by cost. | |
Protected Attributes | |
double | ballRadiusConst_ |
Shrink rate of radius the planner uses to find near neighbors and rewire. | |
double | ballRadiusMax_ |
Maximum radius the planner uses to find near neighbors and rewire. | |
bool | delayCC_ |
Option to delay and reduce collision checking within iterations. | |
double | goalBias_ |
The fraction of time the goal is picked as the state to expand towards (if such a state is available). | |
double | maxDistance_ |
The maximum length of a motion to be added to a tree. | |
boost::shared_ptr < NearestNeighbors< Motion * > > | nn_ |
A nearest-neighbors datastructure containing the tree of motions. | |
RNG | rng_ |
The random number generator. | |
base::StateSamplerPtr | sampler_ |
State sampler. | |
bool | terminate_ |
Option to terminate planning when a solution is found. |
Optimal Rapidly-exploring Random Trees.
Definition at line 77 of file RRTstar.h.
ompl::geometric::RRTstar::RRTstar | ( | const base::SpaceInformationPtr & | si | ) | [inline] |
virtual ompl::geometric::RRTstar::~RRTstar | ( | void | ) | [inline, virtual] |
virtual void ompl::geometric::RRTstar::clear | ( | void | ) | [virtual] |
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() will ignore all previous work.
Reimplemented from ompl::base::Planner.
void ompl::geometric::RRTstar::freeMemory | ( | void | ) | [protected] |
Free the memory allocated by this planner.
double ompl::geometric::RRTstar::getBallRadiusConstant | ( | void | ) | const [inline] |
bool ompl::geometric::RRTstar::getDelayCC | ( | void | ) | const [inline] |
double ompl::geometric::RRTstar::getGoalBias | ( | void | ) | const [inline] |
double ompl::geometric::RRTstar::getMaxBallRadius | ( | void | ) | const [inline] |
virtual void ompl::geometric::RRTstar::getPlannerData | ( | base::PlannerData & | data | ) | const [virtual] |
Get information about the current run of the motion planner. Repeated calls to this function will update data (only additions are made). This is useful to see what changed in the exploration datastructure, between calls to solve(), for example (without calling clear() in between).
Reimplemented from ompl::base::Planner.
double ompl::geometric::RRTstar::getRange | ( | void | ) | const [inline] |
bool ompl::geometric::RRTstar::getTerminate | ( | void | ) | const [inline] |
void ompl::geometric::RRTstar::setBallRadiusConstant | ( | double | ballRadiusConstant | ) | [inline] |
When the planner attempts to rewire the tree, it does so by looking at some of the neighbors within a computed radius. The computation of that radius depends on the multiplicative factor set here. Set this parameter should be set at least to the side length of the (bounded) state space. E.g., if the state space is a box with side length L, then this parameter should be set to at least L for rapid and efficient convergence in trajectory space.
void ompl::geometric::RRTstar::setDelayCC | ( | bool | delayCC | ) | [inline] |
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cost. The planner then goes through this list, starting with the lowest cost, checking for collisions in order to find a parent. The planner stops iterating through the list when a collision free parent is found. This prevents the planner from collsion checking each neighbor, reducing computation time in scenarios where collision checking procedures are expensive.
void ompl::geometric::RRTstar::setGoalBias | ( | double | goalBias | ) | [inline] |
Set the goal bias.
In the process of randomly selecting states in the state space to attempt to go towards, the algorithm may in fact choose the actual goal state, if it knows it, with some probability. This probability is a real number between 0.0 and 1.0; its value should usually be around 0.05 and should not be too large. It is probably a good idea to use the default value.
void ompl::geometric::RRTstar::setMaxBallRadius | ( | double | maxBallRadius | ) | [inline] |
When the planner attempts to rewire the tree, it does so by looking at some of the neighbors within a computed radius. That radius is bounded by the value set here. This parameter should ideally be equal longest straight line from the initial state to anywhere in the state space. In other words, this parameter should be "sqrt(d) L", where d is the dimensionality of space and L is the side length of a box containing the obstacle free space.
void ompl::geometric::RRTstar::setNearestNeighbors | ( | void | ) | [inline] |
void ompl::geometric::RRTstar::setRange | ( | double | distance | ) | [inline] |
void ompl::geometric::RRTstar::setTerminate | ( | bool | terminate | ) | [inline] |
virtual void ompl::geometric::RRTstar::setup | ( | void | ) | [virtual] |
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceInformation::setup() if needed. This must be called before solving.
Reimplemented from ompl::base::Planner.
virtual bool ompl::geometric::RRTstar::solve | ( | const base::PlannerTerminationCondition & | ptc | ) | [virtual] |
Function that can solve the motion planning problem. This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work more time on an unsolved problem, for example. If this option is used, it is assumed the problem definition is not changed (unpredictable results otherwise). The only change in the problem definition that is accounted for is the addition of starting or goal states (but not changing previously added start/goal states). The function terminates if the call to ptc returns true.
Implements ompl::base::Planner.
double ompl::geometric::RRTstar::ballRadiusConst_ [protected] |
double ompl::geometric::RRTstar::ballRadiusMax_ [protected] |
bool ompl::geometric::RRTstar::delayCC_ [protected] |
double ompl::geometric::RRTstar::goalBias_ [protected] |
double ompl::geometric::RRTstar::maxDistance_ [protected] |
boost::shared_ptr< NearestNeighbors<Motion*> > ompl::geometric::RRTstar::nn_ [protected] |
RNG ompl::geometric::RRTstar::rng_ [protected] |
bool ompl::geometric::RRTstar::terminate_ [protected] |