TRRT.h
88 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &plannerTerminationCondition);
300 // *********************************************************************************************************
302 // *********************************************************************************************************
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
double getFrontierThreshold(void) const
Get the distance between a new state and the nearest neighbor that qualifies that state as being a fr...
Definition: TRRT.h:185
void setKConstant(double kConstant)
Set the constant value used to normalize the expression.
Definition: TRRT.h:205
double initTemperature_
A very low value at initialization to authorize very easy positive slopes.
Definition: TRRT.h:326
boost::shared_ptr< NearestNeighbors< Motion * > > nearestNeighbors_
A nearest-neighbors datastructure containing the tree of motions.
Definition: TRRT.h:283
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...
Definition: TRRT.h:309
double getFrontierNodeRatio(void) const
Get the ratio between adding nonfrontier nodes to frontier nodes, for example .1 is 1/10 or one nonfr...
Definition: TRRT.h:199
void setMinTemperature(double minTemperature)
Set the minimum the temperature can drop to before being floored at that value.
Definition: TRRT.h:153
void setTempChangeFactor(double tempChangeFactor)
Set the factor by which the temperature rises or falls based on current acceptance/rejection rate...
Definition: TRRT.h:141
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
bool transitionTest(double childCost, double parentCost, double distance)
Filter irrelevant configuration regarding the search of low-cost paths before inserting into tree...
Definition: TRRT.cpp:421
double getKConstant(void) const
Get the constant value used to normalize the expression.
Definition: TRRT.h:211
double frontierThreshold_
The distance between an old state and a new state that qualifies it as a frontier state...
Definition: TRRT.h:339
double kConstant_
Constant value used to normalize expression. Based on order of magnitude of the considered costs...
Definition: TRRT.h:314
double getInitTemperature(void) const
Get the initial temperature at the beginning of the algorithm. Should be low.
Definition: TRRT.h:171
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: TRRT.h:295
ompl::base::OptimizationObjectivePtr opt_
The optimization objective being optimized by TRRT.
Definition: TRRT.h:345
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: TRRT.cpp:400
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
Definition: TRRT.h:264
double getTempChangeFactor(void) const
Get the factor by which the temperature rises or falls based on current acceptance/rejection rate...
Definition: TRRT.h:147
void setMaxStatesFailed(double maxStatesFailed)
Set the maximum number of states that can be rejected before the temperature starts to rise...
Definition: TRRT.h:129
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...
Definition: TRRT.cpp:174
double getMinTemperature(void) const
Get the minimum the temperature can drop to before being floored at that value.
Definition: TRRT.h:159
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: TRRT.h:286
double getMaxStatesFailed(void) const
Get the maximum number of states that can be rejected before the temperature starts to rise...
Definition: TRRT.h:135
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...
Definition: TRRT.h:192
bool minExpansionControl(double randMotionDistance)
Use ratio to prefer frontier nodes to nonfrontier ones.
Definition: TRRT.cpp:479
void setInitTemperature(double initTemperature)
Set the initial temperature at the beginning of the algorithm. Should be low.
Definition: TRRT.h:165
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state.
Definition: TRRT.h:241
double nonfrontierCount_
Ratio counters for nodes that expand the search space versus those that do not.
Definition: TRRT.h:335
double tempChangeFactor_
Failure temperature factor used when max_num_failed_ failures occur.
Definition: TRRT.h:320
double frontierNodeRatio_
Target ratio of nonfrontier nodes to frontier nodes. rho.
Definition: TRRT.h:342
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: TRRT.cpp:96
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: TRRT.cpp:80
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
void setFrontierThreshold(double frontier_threshold)
Set the distance between a new state and the nearest neighbor that qualifies that state as being a fr...
Definition: TRRT.h:178