00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/contrib/rrt_star/RRTstar.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <algorithm>
00042 #include <limits>
00043 #include <map>
00044
00045 ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "RRTstar")
00046 {
00047 specs_.approximateSolutions = true;
00048 specs_.optimizingPaths = true;
00049
00050 goalBias_ = 0.05;
00051 maxDistance_ = 0.0;
00052 ballRadiusMax_ = 0.0;
00053 ballRadiusConst_ = 1.0;
00054 delayCC_ = true;
00055
00056 Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange);
00057 Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias);
00058 Planner::declareParam<double>("ball_radius_constant", this, &RRTstar::setBallRadiusConstant, &RRTstar::getBallRadiusConstant);
00059 Planner::declareParam<double>("max_ball_radius", this, &RRTstar::setMaxBallRadius, &RRTstar::getMaxBallRadius);
00060 Planner::declareParam<bool>("delay_cc", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC);
00061 }
00062
00063 ompl::geometric::RRTstar::~RRTstar(void)
00064 {
00065 freeMemory();
00066 }
00067
00068 void ompl::geometric::RRTstar::setup(void)
00069 {
00070 Planner::setup();
00071 tools::SelfConfig sc(si_, getName());
00072 sc.configurePlannerRange(maxDistance_);
00073
00074 ballRadiusMax_ = si_->getMaximumExtent();
00075 ballRadiusConst_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
00076
00077 delayCC_ = true;
00078
00079 if (!nn_)
00080 nn_.reset(new NearestNeighborsGNAT<Motion*>());
00081 nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
00082 }
00083
00084 void ompl::geometric::RRTstar::clear(void)
00085 {
00086 Planner::clear();
00087 sampler_.reset();
00088 freeMemory();
00089 if (nn_)
00090 nn_->clear();
00091 }
00092
00093 bool ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
00094 {
00095 checkValidity();
00096 base::Goal *goal = pdef_->getGoal().get();
00097 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00098
00099 if (!goal)
00100 {
00101 msg_.error("Goal undefined");
00102 return false;
00103 }
00104
00105 while (const base::State *st = pis_.nextStart())
00106 {
00107 Motion *motion = new Motion(si_);
00108 si_->copyState(motion->state, st);
00109 nn_->add(motion);
00110 }
00111
00112 if (nn_->size() == 0)
00113 {
00114 msg_.error("There are no valid initial states!");
00115 return false;
00116 }
00117
00118 if (!sampler_)
00119 sampler_ = si_->allocStateSampler();
00120
00121 msg_.inform("Starting with %u states", nn_->size());
00122
00123 Motion *solution = NULL;
00124 Motion *approximation = NULL;
00125 double approximatedist = std::numeric_limits<double>::infinity();
00126 bool sufficientlyShort = false;
00127
00128 Motion *rmotion = new Motion(si_);
00129 base::State *rstate = rmotion->state;
00130 base::State *xstate = si_->allocState();
00131 std::vector<Motion*> solCheck;
00132 std::vector<Motion*> nbh;
00133 std::vector<double> dists;
00134 std::vector<int> valid;
00135 unsigned int rewireTest = 0;
00136
00137 while (ptc() == false)
00138 {
00139
00140 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00141 goal_s->sampleGoal(rstate);
00142 else
00143 sampler_->sampleUniform(rstate);
00144
00145
00146 Motion *nmotion = nn_->nearest(rmotion);
00147
00148 base::State *dstate = rstate;
00149
00150
00151 double d = si_->distance(nmotion->state, rstate);
00152 if (d > maxDistance_)
00153 {
00154 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00155 dstate = xstate;
00156 }
00157
00158 if (si_->checkMotion(nmotion->state, dstate))
00159 {
00160
00161 double distN = si_->distance(dstate, nmotion->state);
00162 Motion *motion = new Motion(si_);
00163 si_->copyState(motion->state, dstate);
00164 motion->parent = nmotion;
00165 motion->cost = nmotion->cost + distN;
00166
00167
00168 double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
00169 ballRadiusMax_);
00170
00171 nn_->nearestR(motion, r, nbh);
00172 rewireTest += nbh.size();
00173
00174
00175 dists.resize(nbh.size());
00176
00177 valid.resize(nbh.size());
00178 std::fill(valid.begin(), valid.end(), 0);
00179
00180 if(delayCC_)
00181 {
00182
00183 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00184 nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
00185
00186
00187 std::sort(nbh.begin(), nbh.end(), compareMotion);
00188
00189 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00190 {
00191 dists[i] = si_->distance(nbh[i]->state, dstate);
00192 nbh[i]->cost -= dists[i];
00193 }
00194
00195
00196 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00197 {
00198 if (nbh[i] != nmotion)
00199 {
00200 double c = nbh[i]->cost + dists[i];
00201 if (c < motion->cost)
00202 {
00203 if (si_->checkMotion(nbh[i]->state, dstate))
00204 {
00205 motion->cost = c;
00206 motion->parent = nbh[i];
00207 valid[i] = 1;
00208 break;
00209 }
00210 else
00211 valid[i] = -1;
00212 }
00213 }
00214 else
00215 {
00216 valid[i] = 1;
00217 dists[i] = distN;
00218 break;
00219 }
00220 }
00221 }
00222 else
00223 {
00224
00225 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00226 {
00227 if (nbh[i] != nmotion)
00228 {
00229 dists[i] = si_->distance(nbh[i]->state, dstate);
00230 double c = nbh[i]->cost + dists[i];
00231 if (c < motion->cost)
00232 {
00233 if (si_->checkMotion(nbh[i]->state, dstate))
00234 {
00235 motion->cost = c;
00236 motion->parent = nbh[i];
00237 valid[i] = 1;
00238 }
00239 else
00240 valid[i] = -1;
00241 }
00242 }
00243 else
00244 {
00245 valid[i] = 1;
00246 dists[i] = distN;
00247 }
00248 }
00249 }
00250
00251
00252 nn_->add(motion);
00253 motion->parent->children.push_back(motion);
00254
00255 solCheck.resize(1);
00256 solCheck[0] = motion;
00257
00258
00259 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00260 if (nbh[i] != motion->parent)
00261 {
00262 double c = motion->cost + dists[i];
00263 if (c < nbh[i]->cost)
00264 {
00265 bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
00266 if (v)
00267 {
00268
00269 removeFromParent (nbh[i]);
00270 double delta = c - nbh[i]->cost;
00271
00272
00273 nbh[i]->parent = motion;
00274 nbh[i]->cost = c;
00275 nbh[i]->parent->children.push_back(nbh[i]);
00276 solCheck.push_back(nbh[i]);
00277
00278
00279 updateChildCosts(nbh[i], delta);
00280 }
00281 }
00282 }
00283
00284
00285 for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00286 {
00287 double dist = 0.0;
00288 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00289 sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00290
00291 if (solved)
00292 {
00293 if (sufficientlyShort)
00294 {
00295 solution = solCheck[i];
00296 break;
00297 }
00298 else if (!solution || (solCheck[i]->cost < solution->cost))
00299 {
00300 solution = solCheck[i];
00301 }
00302 }
00303 else if (!solution && dist < approximatedist)
00304 {
00305 approximation = solCheck[i];
00306 approximatedist = dist;
00307 }
00308 }
00309 }
00310
00311
00312 if (solution && sufficientlyShort)
00313 break;
00314 }
00315
00316 double solutionCost;
00317 bool approximate = (solution == NULL);
00318 bool addedSolution = false;
00319 if (approximate)
00320 {
00321 solution = approximation;
00322 solutionCost = approximatedist;
00323 }
00324 else
00325 solutionCost = solution->cost;
00326
00327 if (solution != NULL)
00328 {
00329
00330 std::vector<Motion*> mpath;
00331 while (solution != NULL)
00332 {
00333 mpath.push_back(solution);
00334 solution = solution->parent;
00335 }
00336
00337
00338 PathGeometric *path = new PathGeometric(si_);
00339 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00340 path->append(mpath[i]->state);
00341 goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
00342 addedSolution = true;
00343 }
00344
00345 si_->freeState(xstate);
00346 if (rmotion->state)
00347 si_->freeState(rmotion->state);
00348 delete rmotion;
00349
00350 msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00351
00352 return addedSolution;
00353 }
00354
00355 void ompl::geometric::RRTstar::removeFromParent(Motion *m)
00356 {
00357 std::vector<Motion*>::iterator it = m->parent->children.begin ();
00358 while (it != m->parent->children.end ())
00359 {
00360 if (*it == m)
00361 {
00362 it = m->parent->children.erase(it);
00363 it = m->parent->children.end ();
00364 }
00365 else
00366 ++it;
00367 }
00368 }
00369
00370 void ompl::geometric::RRTstar::updateChildCosts(Motion *m, double delta)
00371 {
00372 for (size_t i = 0; i < m->children.size(); ++i)
00373 {
00374 m->children[i]->cost += delta;
00375 updateChildCosts(m->children[i], delta);
00376 }
00377 }
00378
00379 void ompl::geometric::RRTstar::freeMemory(void)
00380 {
00381 if (nn_)
00382 {
00383 std::vector<Motion*> motions;
00384 nn_->list(motions);
00385 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00386 {
00387 if (motions[i]->state)
00388 si_->freeState(motions[i]->state);
00389 delete motions[i];
00390 }
00391 }
00392 }
00393
00394 void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
00395 {
00396 Planner::getPlannerData(data);
00397
00398 std::vector<Motion*> motions;
00399 if (nn_)
00400 nn_->list(motions);
00401
00402 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00403 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00404 }