37 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_TRRT_
38 #define OMPL_GEOMETRIC_PLANNERS_RRT_TRRT_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 #include "ompl/base/OptimizationObjective.h"
90 virtual void clear(
void);
217 template<
template<
typename T>
class NN>
223 virtual void setup(
void);
274 bool transitionTest(
double childCost,
double parentCost,
double distance );
336 double frontierCount_;
double getRange(void) const
Get the range the planner is using.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
double getFrontierThreshold(void) const
Get the distance between a new state and the nearest neighbor that qualifies that state as being a fr...
void setKConstant(double kConstant)
Set the constant value used to normalize the expression.
double initTemperature_
A very low value at initialization to authorize very easy positive slopes.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
boost::shared_ptr< NearestNeighbors< Motion * > > nearestNeighbors_
A nearest-neighbors datastructure containing the tree of motions.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
unsigned int maxStatesFailed_
Max number of rejections allowed.
A boost shared pointer wrapper for ompl::base::StateSampler.
double temp_
Temperature parameter used to control the difficulty level of transition tests. Low temperatures limi...
void freeMemory(void)
Free the memory allocated by this planner.
double getFrontierNodeRatio(void) const
Get the ratio between adding nonfrontier nodes to frontier nodes, for example .1 is 1/10 or one nonfr...
void setMinTemperature(double minTemperature)
Set the minimum the temperature can drop to before being floored at that value.
Motion * parent
The parent motion in the exploration tree.
void setTempChangeFactor(double tempChangeFactor)
Set the factor by which the temperature rises or falls based on current acceptance/rejection rate...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
RNG rng_
The random number generator.
double maxDistance_
The maximum length of a motion to be added to a tree.
bool transitionTest(double childCost, double parentCost, double distance)
Filter irrelevant configuration regarding the search of low-cost paths before inserting into tree...
double getKConstant(void) const
Get the constant value used to normalize the expression.
double frontierThreshold_
The distance between an old state and a new state that qualifies it as a frontier state...
double kConstant_
Constant value used to normalize expression. Based on order of magnitude of the considered costs...
double getInitTemperature(void) const
Get the initial temperature at the beginning of the algorithm. Should be low.
TRRT(const base::SpaceInformationPtr &si)
Constructor.
void setGoalBias(double goalBias)
Set the goal bias.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
void setNearestNeighbors(void)
Set a different nearest neighbors datastructure.
Representation of a motion.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Base class for a planner.
base::Cost cost
Cost of the state.
ompl::base::OptimizationObjectivePtr opt_
The optimization objective being optimized by TRRT.
void setRange(double distance)
Set the range the planner is supposed to use.
base::State * state
The state contained by the motion.
A class to store the exit status of Planner::solve()
base::StateSamplerPtr sampler_
State sampler.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
double getTempChangeFactor(void) const
Get the factor by which the temperature rises or falls based on current acceptance/rejection rate...
void setMaxStatesFailed(double maxStatesFailed)
Set the maximum number of states that can be rejected before the temperature starts to rise...
Definition of an abstract state.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &plannerTerminationCondition)
Function that can solve the motion planning problem. This function can be called multiple times on th...
double getMinTemperature(void) const
Get the minimum the temperature can drop to before being floored at that value.
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
double getMaxStatesFailed(void) const
Get the maximum number of states that can be rejected before the temperature starts to rise...
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
void setFrontierNodeRatio(double frontierNodeRatio)
Set the ratio between adding nonfrontier nodes to frontier nodes, for example .1 is 1/10 or one nonfr...
double minTemperature_
Prevent temperature from dropping too far.
bool minExpansionControl(double randMotionDistance)
Use ratio to prefer frontier nodes to nonfrontier ones.
void setInitTemperature(double initTemperature)
Set the initial temperature at the beginning of the algorithm. Should be low.
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Transition-based Rapidly-exploring Random Trees.
bool verbose_
Output debug info.
double getGoalBias(void) const
Get the goal bias the planner is using.
double nonfrontierCount_
Ratio counters for nodes that expand the search space versus those that do not.
SpaceInformationPtr si_
The space information for which planning is done.
double tempChangeFactor_
Failure temperature factor used when max_num_failed_ failures occur.
double frontierNodeRatio_
Target ratio of nonfrontier nodes to frontier nodes. rho.
unsigned int numStatesFailed_
Failure counter for states that are rejected.
void setFrontierThreshold(double frontier_threshold)
Set the distance between a new state and the nearest neighbor that qualifies that state as being a fr...