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 ballRadiusMax_ = si_->getMaximumExtent();
77 ballRadiusConst_ = maxDistance_ * sqrt((
double)si_->getStateSpace()->getDimension());
111 si_->copyState(motion->
state, st);
115 if (nn_->size() == 0)
117 logError(
"There are no valid initial states!");
122 sampler_ = si_->allocStateSampler();
124 logInform(
"Starting with %u states", nn_->size());
127 Motion *approximation = NULL;
128 double approximatedist = std::numeric_limits<double>::infinity();
129 bool sufficientlyShort =
false;
136 std::vector<Motion*> solCheck;
137 std::vector<Motion*> nbh;
138 std::vector<double> dists;
139 std::vector<int> valid;
140 long unsigned int rewireTest = 0;
141 double stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
143 std::pair<base::State*,double> lastValid(tstate, 0.0);
145 while (ptc() ==
false)
147 bool rejected =
false;
153 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
156 sampler_->sampleUniform(rstate);
159 if (inVolume(rstate))
164 if(!si_->isValid(rstate))
168 toTrim = nn_->nearest(rmotion);
169 double newRad = si_->distance(toTrim->
state, rstate);
170 if (newRad < toTrim->volRadius)
183 Motion *nmotion = nn_->nearest(rmotion);
188 double d = si_->distance(nmotion->
state, rstate);
189 if (d > maxDistance_)
191 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
195 if (si_->checkMotion(nmotion->
state, dstate, lastValid))
198 double distN = si_->distance(dstate, nmotion->
state);
200 si_->copyState(motion->
state, dstate);
202 motion->
cost = nmotion->
cost + distN;
205 double r = std::min(ballRadiusConst_ * pow(
log((
double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
208 nn_->nearestR(motion, r, nbh);
209 rewireTest += nbh.size();
212 dists.resize(nbh.size());
214 valid.resize(nbh.size());
215 std::fill(valid.begin(), valid.end(), 0);
220 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
221 if (nbh[i] != nmotion)
223 double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
228 std::sort(nbh.begin(), nbh.end(), compareMotion);
230 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
231 if (nbh[i] != nmotion)
233 dists[i] = si_->distance(nbh[i]->state, dstate);
234 nbh[i]->cost -= dists[i];
237 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
238 if (nbh[i] != nmotion)
241 dists[i] = si_->distance(nbh[i]->state, dstate);
242 double c = nbh[i]->cost + dists[i];
243 if (c < motion->cost)
245 if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
256 double nR = si_->distance(nbh[i]->state, lastValid.first);
257 if (nR < nbh[i]->volRadius)
258 nbh[i]->volRadius = nR;
272 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
273 if (nbh[i] != nmotion)
276 dists[i] = si_->distance(nbh[i]->state, dstate);
277 double c = nbh[i]->cost + dists[i];
278 if (c < motion->cost)
280 if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
290 double newR = si_->distance(nbh[i]->state, lastValid.first);
291 if (newR < nbh[i]->volRadius)
292 nbh[i]->volRadius = newR;
309 solCheck[0] = motion;
312 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
313 if (nbh[i] != motion->
parent)
315 double c = motion->
cost + dists[i];
316 if (c < nbh[i]->cost)
321 if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
324 double R = si_->distance(nbh[i]->state, lastValid.first);
325 if (R < nbh[i]->volRadius)
326 nbh[i]->volRadius = R;
339 removeFromParent (nbh[i]);
340 double delta = c - nbh[i]->cost;
342 nbh[i]->parent = motion;
344 nbh[i]->parent->children.push_back(nbh[i]);
345 solCheck.push_back(nbh[i]);
348 updateChildCosts(nbh[i], delta);
355 solCheck.push_back(solution);
358 for (
unsigned int i = 0 ; i < solCheck.size() ; ++i)
361 bool solved = goal->
isSatisfied(solCheck[i]->state, &dist);
366 if (sufficientlyShort)
368 solution = solCheck[i];
371 else if (!solution || (solCheck[i]->cost < solution->cost))
373 solution = solCheck[i];
376 else if (!solution && dist < approximatedist)
378 approximation = solCheck[i];
379 approximatedist = dist;
384 if (solution && sufficientlyShort)
390 toTrim = nn_->nearest(nmotion);
391 double newRadius = si_->distance(toTrim->
state, lastValid.first);
392 if (newRadius < toTrim->volRadius)
398 bool approximate = (solution == NULL);
399 bool addedSolution =
false;
402 solution = approximation;
403 solutionCost = approximatedist;
406 solutionCost = solution->
cost;
408 if (solution != NULL)
411 std::vector<Motion*> mpath;
412 while (solution != NULL)
414 mpath.push_back(solution);
415 solution = solution->parent;
420 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
421 path->
append(mpath[i]->state);
422 pdef_->addSolutionPath(
base::PathPtr(path), approximate, solutionCost);
423 addedSolution =
true;
426 si_->freeState(xstate);
428 si_->freeState(rmotion->
state);
431 logInform(
"Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
453 for (
size_t i = 0; i < m->
children.size(); ++i)
456 updateChildCosts(m->
children[i], delta);
464 std::vector<Motion*> motions;
466 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
468 if (motions[i]->state)
469 si_->freeState(motions[i]->state);
477 Planner::getPlannerData(data);
479 std::vector<Motion*> motions;
483 for (
unsigned int i = 0 ; i < motions.size() ; ++i)