TRRT.cpp
55 Planner::declareParam<double>("goal_bias", this, &TRRT::setGoalBias, &TRRT::getGoalBias, "0.:.05:1.");
66 Planner::declareParam<unsigned int>("max_states_failed", this, &TRRT::setMaxStatesFailed, &TRRT::getMaxStatesFailed, "1:1000");
67 Planner::declareParam<double>("temp_change_factor", this, &TRRT::setTempChangeFactor, &TRRT::getTempChangeFactor,"0.:.1:10.");
68 Planner::declareParam<double>("min_temperature", this, &TRRT::setMinTemperature, &TRRT::getMinTemperature);
69 Planner::declareParam<double>("init_temperature", this, &TRRT::setInitTemperature, &TRRT::getInitTemperature);
70 Planner::declareParam<double>("frontier_threshold", this, &TRRT::setFrontierThreshold, &TRRT::getFrontierThreshold);
71 Planner::declareParam<double>("frontierNodeRatio", this, &TRRT::setFrontierNodeRatio, &TRRT::getFrontierNodeRatio);
145 nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
174 ompl::geometric::TRRT::solve(const base::PlannerTerminationCondition &plannerTerminationCondition)
183 // Input States ---------------------------------------------------------------------------------
213 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nearestNeighbors_->size());
216 // Solver variables ------------------------------------------------------------------------------------
237 base::State *interpolatedState = si_->allocState(); // Allocates "space information"-sized memory for a state
241 // Begin sampling --------------------------------------------------------------------------------------
271 // Computes the state that lies at time t in [0, 1] on the segment that connects *from* state to *to* state.
272 // The memory location of *state* is not required to be different from the memory of either *from* or *to*.
292 // this stage integrates collision detections in the presence of obstacles and checks for collisions
306 // A possible side effect may appear when the tree expansion toward unexplored regions remains slow, and the
354 // Finish solution processing --------------------------------------------------------------------
388 // Clean up ---------------------------------------------------------------------------------------
421 bool ompl::geometric::TRRT::transitionTest(double childCost, double parentCost, double distance)
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
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
An optimization objective which defines path cost using the idea of mechanical work. To be used in conjunction with TRRT.
Definition: MechanicalWorkOptimizationObjective.h:47
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition: PlannerData.cpp:434
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
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
Definition: PathGeometric.cpp:432
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
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
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
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:49
double getInitTemperature(void) const
Get the initial temperature at the beginning of the algorithm. Should be low.
Definition: TRRT.h:171
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: TRRT.h:295
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
Definition: PlannerData.cpp:444
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Definition: GoalSampleableRegion.h:70
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
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition: PlannerData.cpp:425
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
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
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
static const double COST_MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION
For cost-based planners it has been observed that smaller ranges are typically suitable. The same range computation strategy is used for all planners, but for cost planners an additional factor (smaller than 1) is multiplied in.
Definition: MagicConstants.h:82
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
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:232
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
Definition: MagicConstants.h:109
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
A boost shared pointer wrapper for ompl::base::Path.