37 #include "ompl/geometric/planners/rrt/TRRT.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include "ompl/tools/config/MagicConstants.h"
74 ompl::geometric::TRRT::~TRRT(
void)
84 if (nearestNeighbors_)
85 nearestNeighbors_->clear();
86 lastGoalMotion_ = NULL;
90 temp_ = initTemperature_;
91 nonfrontierCount_ = 1;
101 double averageCost = si_->averageStateCost(100);
104 if (maxDistance_ < std::numeric_limits<double>::epsilon())
111 if (frontierThreshold_ < std::numeric_limits<double>::epsilon())
113 frontierThreshold_ = si_->getMaximumExtent() * 0.01;
114 OMPL_DEBUG(
"Frontier threshold detected to be %lf", frontierThreshold_);
118 if (kConstant_ < std::numeric_limits<double>::epsilon())
120 kConstant_ = averageCost;
121 OMPL_DEBUG(
"K constant detected to be %lf", kConstant_);
125 if (!nearestNeighbors_)
126 nearestNeighbors_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
132 numStatesFailed_ = 0;
133 temp_ = initTemperature_;
134 nonfrontierCount_ = 1;
141 if (nearestNeighbors_)
143 std::vector<Motion*> motions;
144 nearestNeighbors_->list(motions);
145 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
147 if (motions[i]->state)
148 si_->freeState(motions[i]->state);
170 while (
const base::State *state = pis_.nextStart())
176 si_->copyState(motion->
state, state);
179 motion->
cost = stateValidityChecker->cost(motion->
state);
182 nearestNeighbors_->add(motion);
186 if (nearestNeighbors_->size() == 0)
188 OMPL_ERROR(
"There are no valid initial states!");
194 sampler_ = si_->allocStateSampler();
197 OMPL_INFORM(
"Starting with %u states", nearestNeighbors_->size());
205 Motion *approxSolution = NULL;
207 double approxDifference = std::numeric_limits<double>::infinity();
210 double randMotionDistance;
211 double motionDistance;
221 base::State *interpolatedState = si_->allocState();
226 while (plannerTerminationCondition() ==
false)
231 if (goalRegion && rng_.uniform01() < goalBias_ && goalRegion->
canSample())
239 sampler_->sampleUniform(randState);
245 nearMotion = nearestNeighbors_->nearest(randMotion);
250 randMotionDistance = si_->
distance(nearMotion->
state, randState);
253 if(randMotionDistance > maxDistance_)
257 si_->getStateSpace()->interpolate(nearMotion->
state, randState,
258 maxDistance_ / randMotionDistance, interpolatedState);
261 motionDistance = si_->distance(nearMotion->
state, interpolatedState);
264 newState = interpolatedState;
269 newState = randState;
272 motionDistance = randMotionDistance;
285 if(!si_->checkMotion(nearMotion->
state, newState))
292 if(!minExpansionControl(randMotionDistance))
297 double childCost = stateValidityChecker->cost(newState);
300 if(!transitionTest(childCost, nearMotion->
cost, motionDistance))
309 si_->copyState(motion->
state, newState);
310 motion->
parent = nearMotion;
312 motion->
cost = childCost;
315 nearestNeighbors_->add(motion);
320 double distToGoal = 0.0;
324 approxDifference = distToGoal;
330 if (distToGoal < approxDifference)
332 approxDifference = distToGoal;
333 approxSolution = motion;
342 bool approximate =
false;
345 if (solution == NULL)
347 solution = approxSolution;
352 if (solution != NULL)
354 lastGoalMotion_ = solution;
357 std::vector<Motion*> mpath;
358 while (solution != NULL)
360 mpath.push_back(solution);
361 solution = solution->parent;
366 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
367 path->
append(mpath[i]->state);
369 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxDifference);
375 si_->freeState(interpolatedState);
376 if (randMotion->
state)
377 si_->freeState(randMotion->
state);
380 OMPL_INFORM(
"Created %u states", nearestNeighbors_->size());
387 Planner::getPlannerData(data);
389 std::vector<Motion*> motions;
390 if (nearestNeighbors_)
391 nearestNeighbors_->list(motions);
396 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
398 if (motions[i]->parent == NULL)
409 if(childCost <= parentCost)
413 double costSlope = (childCost - parentCost) / distance;
417 double transitionProbability = 1.;
425 transitionProbability = exp(-costSlope / (kConstant_ * temp_));
429 if(rng_.uniform01() <= transitionProbability)
431 if (temp_ > minTemperature_)
433 temp_ /= tempChangeFactor_;
436 if(temp_ < minTemperature_)
438 temp_ = minTemperature_;
442 numStatesFailed_ = 0;
449 if(numStatesFailed_ >= maxStatesFailed_)
451 temp_ *= tempChangeFactor_;
452 numStatesFailed_ = 0;
467 if(randMotionDistance > frontierThreshold_)
479 if(nonfrontierCount_ / frontierCount_ > frontierNodeRatio_)