37 #include "ompl/contrib/rrt_star/RRTstar.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/datastructures/NearestNeighborsGNAT.h"
40 #include "ompl/tools/config/SelfConfig.h"
45 ompl::geometric::RRTstar::RRTstar(
const base::SpaceInformationPtr &si) : base::Planner(si,
"RRTstar")
63 ompl::geometric::RRTstar::~RRTstar(
void)
74 ballRadiusMax_ = si_->getMaximumExtent();
75 ballRadiusConst_ = maxDistance_ * sqrt((
double)si_->getStateSpace()->getDimension());
108 si_->copyState(motion->
state, st);
112 if (nn_->size() == 0)
114 logError(
"There are no valid initial states!");
119 sampler_ = si_->allocStateSampler();
121 logInform(
"Starting with %u states", nn_->size());
124 Motion *approximation = NULL;
125 double approximatedist = std::numeric_limits<double>::infinity();
126 bool sufficientlyShort =
false;
131 std::vector<Motion*> solCheck;
132 std::vector<Motion*> nbh;
133 std::vector<double> dists;
134 std::vector<int> valid;
135 unsigned int rewireTest = 0;
136 double stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
138 while (ptc() ==
false)
141 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
144 sampler_->sampleUniform(rstate);
147 Motion *nmotion = nn_->nearest(rmotion);
152 double d = si_->distance(nmotion->
state, rstate);
153 if (d > maxDistance_)
155 si_->getStateSpace()->interpolate(nmotion->
state, rstate, maxDistance_ / d, xstate);
159 if (si_->checkMotion(nmotion->
state, dstate))
162 double distN = si_->distance(dstate, nmotion->
state);
164 si_->copyState(motion->
state, dstate);
166 motion->
cost = nmotion->
cost + distN;
169 double r = std::min(ballRadiusConst_ * pow(
log((
double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
172 nn_->nearestR(motion, r, nbh);
173 rewireTest += nbh.size();
176 dists.resize(nbh.size());
178 valid.resize(nbh.size());
179 std::fill(valid.begin(), valid.end(), 0);
184 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
185 nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
188 std::sort(nbh.begin(), nbh.end(), compareMotion);
190 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
192 dists[i] = si_->distance(nbh[i]->state, dstate);
193 nbh[i]->cost -= dists[i];
197 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
199 if (nbh[i] != nmotion)
201 double c = nbh[i]->cost + dists[i];
202 if (c < motion->cost)
204 if (si_->checkMotion(nbh[i]->state, dstate))
226 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
228 if (nbh[i] != nmotion)
230 dists[i] = si_->distance(nbh[i]->state, dstate);
231 double c = nbh[i]->cost + dists[i];
232 if (c < motion->cost)
234 if (si_->checkMotion(nbh[i]->state, dstate))
257 solCheck[0] = motion;
260 for (
unsigned int i = 0 ; i < nbh.size() ; ++i)
261 if (nbh[i] != motion->
parent)
263 double c = motion->
cost + dists[i];
264 if (c < nbh[i]->cost)
266 bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
270 removeFromParent (nbh[i]);
271 double delta = c - nbh[i]->cost;
274 nbh[i]->parent = motion;
276 nbh[i]->parent->children.push_back(nbh[i]);
277 solCheck.push_back(nbh[i]);
280 updateChildCosts(nbh[i], delta);
287 solCheck.push_back(solution);
290 for (
unsigned int i = 0 ; i < solCheck.size() ; ++i)
293 bool solved = goal->
isSatisfied(solCheck[i]->state, &dist);
298 if (sufficientlyShort)
300 solution = solCheck[i];
303 else if (!solution || (solCheck[i]->cost < solution->cost))
305 solution = solCheck[i];
308 else if (!solution && dist < approximatedist)
310 approximation = solCheck[i];
311 approximatedist = dist;
317 if (solution && sufficientlyShort)
322 bool approximate = (solution == NULL);
323 bool addedSolution =
false;
326 solution = approximation;
327 solutionCost = approximatedist;
330 solutionCost = solution->
cost;
332 if (solution != NULL)
335 std::vector<Motion*> mpath;
336 while (solution != NULL)
338 mpath.push_back(solution);
339 solution = solution->parent;
344 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
345 path->
append(mpath[i]->state);
346 pdef_->addSolutionPath(
base::PathPtr(path), approximate, solutionCost);
347 addedSolution =
true;
350 si_->freeState(xstate);
352 si_->freeState(rmotion->
state);
355 logInform(
"Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
377 for (
size_t i = 0; i < m->
children.size(); ++i)
380 updateChildCosts(m->
children[i], delta);
388 std::vector<Motion*> motions;
390 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
392 if (motions[i]->state)
393 si_->freeState(motions[i]->state);
401 Planner::getPlannerData(data);
403 std::vector<Motion*> motions;
407 for (
unsigned int i = 0 ; i < motions.size() ; ++i)