37 #include "ompl/contrib/rrt_star/BallTreeRRTstar.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
40 #include "ompl/tools/config/SelfConfig.h"
45 ompl::geometric::BallTreeRRTstar::BallTreeRRTstar(
const base::SpaceInformationPtr &si) : base::Planner(si,
"BallTreeRRTstar")
54 rO_ = std::numeric_limits<double>::infinity();
65 ompl::geometric::BallTreeRRTstar::~BallTreeRRTstar(
void)
76 if (ballRadiusMax_ == 0.0)
77 ballRadiusMax_ = si_->getMaximumExtent();
78 if (ballRadiusConst_ == 0.0)
79 ballRadiusConst_ = maxDistance_ * sqrt((
double)si_->getStateSpace()->getDimension());
103 if (opt && !dynamic_cast<base::PathLengthOptimizationObjective*>(opt))
106 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());
118 si_->copyState(motion->
state, st);
122 if (nn_->size() == 0)
124 OMPL_ERROR(
"There are no valid initial states!");
129 sampler_ = si_->allocStateSampler();
131 OMPL_INFORM(
"Starting with %u states", nn_->size());
134 Motion *approximation = NULL;
135 double approximatedist = std::numeric_limits<double>::infinity();
136 bool sufficientlyShort =
false;
143 std::vector<Motion*> solCheck;
144 std::vector<Motion*> nbh;
145 std::vector<double> dists;
146 std::vector<int> valid;
147 long unsigned int rewireTest = 0;
148 double stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
150 std::pair<base::State*,double> lastValid(tstate, 0.0);
154 bool rejected =
false;
160 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
163 sampler_->sampleUniform(rstate);
166 if (inVolume(rstate))
171 if(!si_->isValid(rstate))
175 toTrim = nn_->nearest(rmotion);
176 double newRad = si_->distance(toTrim->
state, rstate);
177 if (newRad < toTrim->volRadius)
190 Motion *nmotion = nn_->nearest(rmotion);
195 double d = si_->distance(nmotion->
state, rstate);
196 if (d > maxDistance_)
198 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
202 if (si_->checkMotion(nmotion->
state, dstate, lastValid))
205 double distN = si_->distance(dstate, nmotion->
state);
207 si_->copyState(motion->
state, dstate);
209 motion->
cost = nmotion->
cost + distN;
212 double r = std::min(ballRadiusConst_ * pow(
log((
double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
215 nn_->nearestR(motion, r, nbh);
216 rewireTest += nbh.size();
219 dists.resize(nbh.size());
221 valid.resize(nbh.size());
222 std::fill(valid.begin(), valid.end(), 0);
227 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
228 if (nbh[i] != nmotion)
230 double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
235 std::sort(nbh.begin(), nbh.end(), compareMotion);
237 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
238 if (nbh[i] != nmotion)
240 dists[i] = si_->distance(nbh[i]->state, dstate);
241 nbh[i]->cost -= dists[i];
244 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
245 if (nbh[i] != nmotion)
248 dists[i] = si_->distance(nbh[i]->state, dstate);
249 double c = nbh[i]->cost + dists[i];
250 if (c < motion->cost)
252 if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
263 double nR = si_->distance(nbh[i]->state, lastValid.first);
264 if (nR < nbh[i]->volRadius)
265 nbh[i]->volRadius = nR;
279 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
280 if (nbh[i] != nmotion)
283 dists[i] = si_->distance(nbh[i]->state, dstate);
284 double c = nbh[i]->cost + dists[i];
285 if (c < motion->cost)
287 if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
297 double newR = si_->distance(nbh[i]->state, lastValid.first);
298 if (newR < nbh[i]->volRadius)
299 nbh[i]->volRadius = newR;
316 solCheck[0] = motion;
319 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
320 if (nbh[i] != motion->
parent)
322 double c = motion->
cost + dists[i];
323 if (c < nbh[i]->cost)
328 if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
331 double R = si_->distance(nbh[i]->state, lastValid.first);
332 if (R < nbh[i]->volRadius)
333 nbh[i]->volRadius = R;
346 removeFromParent (nbh[i]);
347 double delta = c - nbh[i]->cost;
349 nbh[i]->parent = motion;
351 nbh[i]->parent->children.push_back(nbh[i]);
352 solCheck.push_back(nbh[i]);
355 updateChildCosts(nbh[i], delta);
362 solCheck.push_back(solution);
365 for (
unsigned int i = 0 ; i < solCheck.size() ; ++i)
368 bool solved = goal->
isSatisfied(solCheck[i]->state, &dist);
369 sufficientlyShort = solved ? (opt ? opt->
isSatisfied(solCheck[i]->cost) :
true) :
false;
373 if (sufficientlyShort)
375 solution = solCheck[i];
378 else if (!solution || (solCheck[i]->cost < solution->cost))
380 solution = solCheck[i];
383 else if (!solution && dist < approximatedist)
385 approximation = solCheck[i];
386 approximatedist = dist;
391 if (solution && sufficientlyShort)
397 toTrim = nn_->nearest(nmotion);
398 double newRadius = si_->distance(toTrim->
state, lastValid.first);
399 if (newRadius < toTrim->volRadius)
405 bool approximate = (solution == NULL);
406 bool addedSolution =
false;
409 solution = approximation;
410 solutionCost = approximatedist;
413 solutionCost = solution->
cost;
415 if (solution != NULL)
418 std::vector<Motion*> mpath;
419 while (solution != NULL)
421 mpath.push_back(solution);
422 solution = solution->parent;
427 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
428 path->
append(mpath[i]->state);
429 pdef_->addSolutionPath(
base::PathPtr(path), approximate, solutionCost);
430 addedSolution =
true;
433 si_->freeState(xstate);
435 si_->freeState(rmotion->
state);
438 OMPL_INFORM(
"Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
460 for (
size_t i = 0; i < m->
children.size(); ++i)
463 updateChildCosts(m->
children[i], delta);
471 std::vector<Motion*> motions;
473 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
475 if (motions[i]->state)
476 si_->freeState(motions[i]->state);
484 Planner::getPlannerData(data);
486 std::vector<Motion*> motions;
490 for (
unsigned int i = 0 ; i < motions.size() ; ++i)