TRRT.cpp
51 Planner::declareParam<double>("goal_bias", this, &TRRT::setGoalBias, &TRRT::getGoalBias, "0.:.05:1.");
60 Planner::declareParam<double>("temp_change_factor", this, &TRRT::setTempChangeFactor, &TRRT::getTempChangeFactor,
62 Planner::declareParam<double>("init_temperature", this, &TRRT::setInitTemperature, &TRRT::getInitTemperature);
63 Planner::declareParam<double>("frontier_threshold", this, &TRRT::setFrontierThreshold, &TRRT::getFrontierThreshold);
64 Planner::declareParam<double>("frontierNodeRatio", this, &TRRT::setFrontierNodeRatio, &TRRT::getFrontierNodeRatio);
65 Planner::declareParam<double>("cost_threshold", this, &TRRT::setCostThreshold, &TRRT::getCostThreshold);
97 OMPL_INFORM("%s: No optimization objective specified. Defaulting to mechanical work minimization.",
152 ompl::geometric::TRRT::solve(const base::PlannerTerminationCondition &plannerTerminationCondition)
161 // Input States ---------------------------------------------------------------------------------
197 // Solver variables ------------------------------------------------------------------------------------
217 base::State *interpolatedState = si_->allocState(); // Allocates "space information"-sized memory for a state
221 // Begin sampling --------------------------------------------------------------------------------------
251 // Computes the state that lies at time t in [0, 1] on the segment that connects *from* state to *to* state.
252 // The memory location of *state* is not required to be different from the memory of either *from* or *to*.
253 si_->getStateSpace()->interpolate(nearMotion->state, randState, maxDistance_ / randMotionDistance,
266 // this stage integrates collision detections in the presence of obstacles and checks for collisions
271 // A possible side effect may appear when the tree expansion toward unexplored regions remains slow, and the
293 if (opt_->isCostBetterThan(motion->cost, bestCost_)) // motion->cost is better than the existing best
295 if (opt_->isCostBetterThan(worstCost_, motion->cost)) // motion->cost is worse than the existing worst
319 // Finish solution processing --------------------------------------------------------------------
353 // Clean up ---------------------------------------------------------------------------------------
381 data.addEdge(base::PlannerDataVertex(motion->parent->state), base::PlannerDataVertex(motion->state));
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
base::PlannerStatus solve(const base::PlannerTerminationCondition &plannerTerminationCondition) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: TRRT.cpp:152
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:422
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: TRRT.cpp:365
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:63
void setTempChangeFactor(double factor)
Set the factor by which the temperature is increased after a failed transition test. This value should be in the range (0, 1], typically close to zero (default is 0.1). This value is an exponential (e^factor) that is multiplied with the current temperature.
Definition: TRRT.h:138
double getFrontierNodeRatio() const
Get the ratio between adding nonfrontier nodes to frontier nodes, for example .1 is 1/10 or one nonfr...
Definition: TRRT.h:201
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:213
double frontierThreshold_
The distance between an old state and a new state that qualifies it as a frontier state...
Definition: TRRT.h:318
double getTempChangeFactor() const
Get the factor by which the temperature rises based on current acceptance/rejection rate...
Definition: TRRT.h:144
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:58
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:47
bool transitionTest(const base::Cost &motionCost)
Filter irrelevant configuration regarding the search of low-cost paths before inserting into tree...
Definition: TRRT.cpp:385
base::Cost costThreshold_
All motion costs must be better than this cost (default is infinity)
Definition: TRRT.h:300
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: TRRT.cpp:73
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:432
A shared pointer wrapper for ompl::base::SpaceInformation.
double getCostThreshold() const
Get the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: TRRT.h:160
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:413
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
double getInitTemperature() const
Get the temperature at the start of planning.
Definition: TRRT.h:173
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: TRRT.cpp:90
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:194
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:79
bool minExpansionControl(double randMotionDistance)
Use ratio to prefer frontier nodes to nonfrontier ones.
Definition: TRRT.cpp:412
void setInitTemperature(double initTemperature)
Set the initial temperature at the beginning of the algorithm. Should be high to allow for initial ex...
Definition: TRRT.h:167
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:59
double getFrontierThreshold() const
Get the distance between a new state and the nearest neighbor that qualifies that state as being a fr...
Definition: TRRT.h:187
double frontierNodeRatio_
Target ratio of non-frontier nodes to frontier nodes. rho.
Definition: TRRT.h:321
void setCostThreshold(double maxCost)
Set the cost threshold (default is infinity). Any motion cost that is not better than this cost (acco...
Definition: TRRT.h:152
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:180