37 #include "ompl/contrib/rrt_star/RRTstar.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
44 ompl::geometric::RRTstar::RRTstar(
const base::SpaceInformationPtr &si) : base::Planner(si,
"RRTstar")
62 ompl::geometric::RRTstar::~RRTstar(
void)
73 if (ballRadiusMax_ == 0.0)
74 ballRadiusMax_ = si_->getMaximumExtent();
75 if (ballRadiusConst_ == 0.0)
76 ballRadiusConst_ = maxDistance_ * sqrt((
double)si_->getStateSpace()->getDimension());
79 nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
99 if (opt && !dynamic_cast<base::PathLengthOptimizationObjective*>(opt))
102 OMPL_WARN(
"Optimization objective '%s' specified, but such an objective is not appropriate for %s. Only path length can be optimized.", getName().c_str(), opt->
getDescription().c_str());
114 si_->copyState(motion->
state, st);
118 if (nn_->size() == 0)
120 OMPL_ERROR(
"There are no valid initial states!");
125 sampler_ = si_->allocStateSampler();
127 OMPL_INFORM(
"Starting with %u states", nn_->size());
130 Motion *approximation = NULL;
131 double approximatedist = std::numeric_limits<double>::infinity();
132 bool sufficientlyShort =
false;
137 std::vector<Motion*> solCheck;
138 std::vector<Motion*> nbh;
139 std::vector<double> dists;
140 std::vector<int> valid;
141 unsigned int rewireTest = 0;
142 double stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
147 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
150 sampler_->sampleUniform(rstate);
153 Motion *nmotion = nn_->nearest(rmotion);
158 double d = si_->distance(nmotion->
state, rstate);
159 if (d > maxDistance_)
161 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
165 if (si_->checkMotion(nmotion->
state, dstate))
168 double distN = si_->distance(dstate, nmotion->
state);
170 si_->copyState(motion->
state, dstate);
172 motion->
cost = nmotion->
cost + distN;
175 double r = std::min(ballRadiusConst_ * pow(
log((
double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
178 nn_->nearestR(motion, r, nbh);
179 rewireTest += nbh.size();
182 dists.resize(nbh.size());
184 valid.resize(nbh.size());
185 std::fill(valid.begin(), valid.end(), 0);
190 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
191 nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
194 std::sort(nbh.begin(), nbh.end(), compareMotion);
196 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
198 dists[i] = si_->distance(nbh[i]->state, dstate);
199 nbh[i]->cost -= dists[i];
203 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
205 if (nbh[i] != nmotion)
207 double c = nbh[i]->cost + dists[i];
208 if (c < motion->cost)
210 if (si_->checkMotion(nbh[i]->state, dstate))
232 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
234 if (nbh[i] != nmotion)
236 dists[i] = si_->distance(nbh[i]->state, dstate);
237 double c = nbh[i]->cost + dists[i];
238 if (c < motion->cost)
240 if (si_->checkMotion(nbh[i]->state, dstate))
263 solCheck[0] = motion;
266 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
267 if (nbh[i] != motion->
parent)
269 double c = motion->
cost + dists[i];
270 if (c < nbh[i]->cost)
272 bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
276 removeFromParent (nbh[i]);
277 double delta = c - nbh[i]->cost;
280 nbh[i]->parent = motion;
282 nbh[i]->parent->children.push_back(nbh[i]);
283 solCheck.push_back(nbh[i]);
286 updateChildCosts(nbh[i], delta);
293 solCheck.push_back(solution);
296 for (
unsigned int i = 0 ; i < solCheck.size() ; ++i)
299 bool solved = goal->
isSatisfied(solCheck[i]->state, &dist);
300 sufficientlyShort = solved ? (opt ? opt->
isSatisfied(solCheck[i]->cost) :
true) :
false;
304 if (sufficientlyShort)
306 solution = solCheck[i];
309 else if (!solution || (solCheck[i]->cost < solution->cost))
311 solution = solCheck[i];
314 else if (!solution && dist < approximatedist)
316 approximation = solCheck[i];
317 approximatedist = dist;
323 if (solution && sufficientlyShort)
328 bool approximate = (solution == NULL);
329 bool addedSolution =
false;
332 solution = approximation;
333 solutionCost = approximatedist;
336 solutionCost = solution->
cost;
338 if (solution != NULL)
341 std::vector<Motion*> mpath;
342 while (solution != NULL)
344 mpath.push_back(solution);
345 solution = solution->parent;
350 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
351 path->
append(mpath[i]->state);
352 pdef_->addSolutionPath(
base::PathPtr(path), approximate, solutionCost);
353 addedSolution =
true;
356 si_->freeState(xstate);
358 si_->freeState(rmotion->
state);
361 OMPL_INFORM(
"Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
383 for (
size_t i = 0; i < m->
children.size(); ++i)
386 updateChildCosts(m->
children[i], delta);
394 std::vector<Motion*> motions;
396 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
398 if (motions[i]->state)
399 si_->freeState(motions[i]->state);
407 Planner::getPlannerData(data);
409 std::vector<Motion*> motions;
413 for (
unsigned int i = 0 ; i < motions.size() ; ++i)