37 #include "ompl/geometric/planners/rrt/TRRT.h"
38 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
40 #include "ompl/tools/config/SelfConfig.h"
41 #include "ompl/tools/config/MagicConstants.h"
75 ompl::geometric::TRRT::~TRRT(
void)
85 if (nearestNeighbors_)
86 nearestNeighbors_->clear();
87 lastGoalMotion_ = NULL;
91 temp_ = initTemperature_;
92 nonfrontierCount_ = 1;
101 bool usingDefaultObjective =
false;
102 if (!pdef_->hasOptimizationObjective())
104 OMPL_INFORM(
"%s: No optimization objective specified.", getName().c_str());
105 usingDefaultObjective =
true;
107 else if (!boost::dynamic_pointer_cast<
110 OMPL_INFORM(
"%s: TRRT was supplied an inappropriate optimization objective; it can only handle types of ompl::base::MechanicalWorkOptimizationObjective.", getName().c_str());
111 usingDefaultObjective =
true;
114 if (usingDefaultObjective)
117 OMPL_INFORM(
"%s: Defaulting to optimizing path length.", getName().c_str());
120 opt_ = pdef_->getOptimizationObjective();
123 if (maxDistance_ < std::numeric_limits<double>::epsilon())
130 if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
132 frontierThreshold_ = si_->getMaximumExtent() * 0.01;
133 OMPL_DEBUG(
"%s: Frontier threshold detected to be %lf", getName().c_str(), frontierThreshold_);
137 if (kConstant_ < std::numeric_limits<double>::epsilon())
141 kConstant_ = averageCost;
142 OMPL_DEBUG(
"%s: K constant detected to be %lf", getName().c_str(), kConstant_);
146 if (!nearestNeighbors_)
147 nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
153 numStatesFailed_ = 0;
154 temp_ = initTemperature_;
155 nonfrontierCount_ = 1;
162 if (nearestNeighbors_)
164 std::vector<Motion*> motions;
165 nearestNeighbors_->list(motions);
166 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
168 if (motions[i]->state)
169 si_->freeState(motions[i]->state);
188 while (
const base::State *state = pis_.nextStart())
194 si_->copyState(motion->
state, state);
197 motion->
cost = opt_->stateCost(motion->
state);
200 nearestNeighbors_->add(motion);
204 if (nearestNeighbors_->size() == 0)
206 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
212 sampler_ = si_->allocStateSampler();
215 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), nearestNeighbors_->size());
223 Motion *approxSolution = NULL;
225 double approxDifference = std::numeric_limits<double>::infinity();
228 double randMotionDistance;
229 double motionDistance;
239 base::State *interpolatedState = si_->allocState();
244 while (plannerTerminationCondition() ==
false)
249 if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->
canSample())
257 sampler_->sampleUniform(randState);
263 nearMotion = nearestNeighbors_->nearest(randMotion);
268 randMotionDistance = si_->distance(nearMotion->
state, randState);
271 if (randMotionDistance > maxDistance_)
275 si_->getStateSpace()->interpolate(nearMotion->
state, randState,
276 maxDistance_ / randMotionDistance, interpolatedState);
279 motionDistance = si_->distance(nearMotion->
state, interpolatedState);
282 newState = interpolatedState;
287 newState = randState;
290 motionDistance = randMotionDistance;
303 if (!si_->checkMotion(nearMotion->
state, newState))
310 if (!minExpansionControl(randMotionDistance))
315 base::Cost childCost = opt_->stateCost(newState);
318 if(!transitionTest(childCost.
v, nearMotion->
cost.
v, motionDistance))
327 si_->copyState(motion->
state, newState);
328 motion->
parent = nearMotion;
329 motion->
cost = childCost;
332 nearestNeighbors_->add(motion);
337 double distToGoal = 0.0;
341 approxDifference = distToGoal;
347 if (distToGoal < approxDifference)
349 approxDifference = distToGoal;
350 approxSolution = motion;
359 bool approximate =
false;
362 if (solution == NULL)
364 solution = approxSolution;
369 if (solution != NULL)
371 lastGoalMotion_ = solution;
374 std::vector<Motion*> mpath;
375 while (solution != NULL)
377 mpath.push_back(solution);
378 solution = solution->parent;
383 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
384 path->
append(mpath[i]->state);
386 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxDifference);
392 si_->freeState(interpolatedState);
393 if (randMotion->
state)
394 si_->freeState(randMotion->
state);
397 OMPL_INFORM(
"%s: Created %u states", getName().c_str(), nearestNeighbors_->size());
404 Planner::getPlannerData(data);
406 std::vector<Motion*> motions;
407 if (nearestNeighbors_)
408 nearestNeighbors_->list(motions);
413 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
415 if (motions[i]->parent == NULL)
426 if (childCost <= parentCost)
430 double costSlope = (childCost - parentCost) / distance;
434 double transitionProbability = 1.;
442 transitionProbability = exp(-costSlope / (kConstant_ * temp_));
446 if (rng_.uniform01() <= transitionProbability)
448 if (temp_ > minTemperature_)
450 temp_ /= tempChangeFactor_;
453 if (temp_ < minTemperature_)
455 temp_ = minTemperature_;
459 numStatesFailed_ = 0;
466 if (numStatesFailed_ >= maxStatesFailed_)
468 temp_ *= tempChangeFactor_;
469 numStatesFailed_ = 0;
484 if (randMotionDistance > frontierThreshold_)
496 if ((
double)nonfrontierCount_ / (
double)frontierCount_ > frontierNodeRatio_)
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
double getRange(void) const
Get the range the planner is using.
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
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...
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.
An optimization objective which defines path cost using the idea of mechanical work. To be used in conjunction with TRRT.
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...
void freeMemory(void)
Free the memory allocated by this planner.
Abstract definition of goals.
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...
double v
The value of the cost.
double maxDistance_
The maximum length of a motion to be added to a tree.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
bool transitionTest(double childCost, double parentCost, double distance)
Filter irrelevant configuration regarding the search of low-cost paths before inserting into tree...
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
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.
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...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
Abstract definition of a goal region that can be sampled.
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.
Representation of a motion.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
base::Cost cost
Cost of the state.
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()
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...
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...
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 of an abstract state.
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...
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)...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
double getMaxStatesFailed(void) const
Get the maximum number of states that can be rejected before the temperature starts to rise...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
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.
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.
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.
double getGoalBias(void) const
Get the goal bias the planner is using.
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
Definition of a geometric path.
double tempChangeFactor_
Failure temperature factor used when max_num_failed_ failures occur.
double frontierNodeRatio_
Target ratio of nonfrontier nodes to frontier nodes. rho.
void setFrontierThreshold(double frontier_threshold)
Set the distance between a new state and the nearest neighbor that qualifies that state as being a fr...
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.