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/goals/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 ompl::base::PlannerStatus 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 logError("Goal undefined");
00102 return base::PlannerStatus::INVALID_GOAL;
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 logError("There are no valid initial states!");
00115 return base::PlannerStatus::INVALID_START;
00116 }
00117
00118 if (!sampler_)
00119 sampler_ = si_->allocStateSampler();
00120
00121 logInform("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 double stateSpaceDimensionConstant = 1.0 / (double)si_->getStateSpace()->getDimension();
00137
00138 while (ptc() == false)
00139 {
00140
00141 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00142 goal_s->sampleGoal(rstate);
00143 else
00144 sampler_->sampleUniform(rstate);
00145
00146
00147 Motion *nmotion = nn_->nearest(rmotion);
00148
00149 base::State *dstate = rstate;
00150
00151
00152 double d = si_->distance(nmotion->state, rstate);
00153 if (d > maxDistance_)
00154 {
00155 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00156 dstate = xstate;
00157 }
00158
00159 if (si_->checkMotion(nmotion->state, dstate))
00160 {
00161
00162 double distN = si_->distance(dstate, nmotion->state);
00163 Motion *motion = new Motion(si_);
00164 si_->copyState(motion->state, dstate);
00165 motion->parent = nmotion;
00166 motion->cost = nmotion->cost + distN;
00167
00168
00169 double r = std::min(ballRadiusConst_ * pow(log((double)(1 + nn_->size())) / (double)(nn_->size()), stateSpaceDimensionConstant),
00170 ballRadiusMax_);
00171
00172 nn_->nearestR(motion, r, nbh);
00173 rewireTest += nbh.size();
00174
00175
00176 dists.resize(nbh.size());
00177
00178 valid.resize(nbh.size());
00179 std::fill(valid.begin(), valid.end(), 0);
00180
00181 if(delayCC_)
00182 {
00183
00184 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00185 nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
00186
00187
00188 std::sort(nbh.begin(), nbh.end(), compareMotion);
00189
00190 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00191 {
00192 dists[i] = si_->distance(nbh[i]->state, dstate);
00193 nbh[i]->cost -= dists[i];
00194 }
00195
00196
00197 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00198 {
00199 if (nbh[i] != nmotion)
00200 {
00201 double c = nbh[i]->cost + dists[i];
00202 if (c < motion->cost)
00203 {
00204 if (si_->checkMotion(nbh[i]->state, dstate))
00205 {
00206 motion->cost = c;
00207 motion->parent = nbh[i];
00208 valid[i] = 1;
00209 break;
00210 }
00211 else
00212 valid[i] = -1;
00213 }
00214 }
00215 else
00216 {
00217 valid[i] = 1;
00218 dists[i] = distN;
00219 break;
00220 }
00221 }
00222 }
00223 else
00224 {
00225
00226 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00227 {
00228 if (nbh[i] != nmotion)
00229 {
00230 dists[i] = si_->distance(nbh[i]->state, dstate);
00231 double c = nbh[i]->cost + dists[i];
00232 if (c < motion->cost)
00233 {
00234 if (si_->checkMotion(nbh[i]->state, dstate))
00235 {
00236 motion->cost = c;
00237 motion->parent = nbh[i];
00238 valid[i] = 1;
00239 }
00240 else
00241 valid[i] = -1;
00242 }
00243 }
00244 else
00245 {
00246 valid[i] = 1;
00247 dists[i] = distN;
00248 }
00249 }
00250 }
00251
00252
00253 nn_->add(motion);
00254 motion->parent->children.push_back(motion);
00255
00256 solCheck.resize(1);
00257 solCheck[0] = motion;
00258
00259
00260 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00261 if (nbh[i] != motion->parent)
00262 {
00263 double c = motion->cost + dists[i];
00264 if (c < nbh[i]->cost)
00265 {
00266 bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
00267 if (v)
00268 {
00269
00270 removeFromParent (nbh[i]);
00271 double delta = c - nbh[i]->cost;
00272
00273
00274 nbh[i]->parent = motion;
00275 nbh[i]->cost = c;
00276 nbh[i]->parent->children.push_back(nbh[i]);
00277 solCheck.push_back(nbh[i]);
00278
00279
00280 updateChildCosts(nbh[i], delta);
00281 }
00282 }
00283 }
00284
00285
00286 if (solution)
00287 solCheck.push_back(solution);
00288
00289
00290 for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00291 {
00292 double dist = 0.0;
00293 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00294 sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00295
00296 if (solved)
00297 {
00298 if (sufficientlyShort)
00299 {
00300 solution = solCheck[i];
00301 break;
00302 }
00303 else if (!solution || (solCheck[i]->cost < solution->cost))
00304 {
00305 solution = solCheck[i];
00306 }
00307 }
00308 else if (!solution && dist < approximatedist)
00309 {
00310 approximation = solCheck[i];
00311 approximatedist = dist;
00312 }
00313 }
00314 }
00315
00316
00317 if (solution && sufficientlyShort)
00318 break;
00319 }
00320
00321 double solutionCost;
00322 bool approximate = (solution == NULL);
00323 bool addedSolution = false;
00324 if (approximate)
00325 {
00326 solution = approximation;
00327 solutionCost = approximatedist;
00328 }
00329 else
00330 solutionCost = solution->cost;
00331
00332 if (solution != NULL)
00333 {
00334
00335 std::vector<Motion*> mpath;
00336 while (solution != NULL)
00337 {
00338 mpath.push_back(solution);
00339 solution = solution->parent;
00340 }
00341
00342
00343 PathGeometric *path = new PathGeometric(si_);
00344 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00345 path->append(mpath[i]->state);
00346 pdef_->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
00347 addedSolution = true;
00348 }
00349
00350 si_->freeState(xstate);
00351 if (rmotion->state)
00352 si_->freeState(rmotion->state);
00353 delete rmotion;
00354
00355 logInform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00356
00357 return base::PlannerStatus(addedSolution, approximate);
00358 }
00359
00360 void ompl::geometric::RRTstar::removeFromParent(Motion *m)
00361 {
00362 std::vector<Motion*>::iterator it = m->parent->children.begin ();
00363 while (it != m->parent->children.end ())
00364 {
00365 if (*it == m)
00366 {
00367 it = m->parent->children.erase(it);
00368 it = m->parent->children.end ();
00369 }
00370 else
00371 ++it;
00372 }
00373 }
00374
00375 void ompl::geometric::RRTstar::updateChildCosts(Motion *m, double delta)
00376 {
00377 for (size_t i = 0; i < m->children.size(); ++i)
00378 {
00379 m->children[i]->cost += delta;
00380 updateChildCosts(m->children[i], delta);
00381 }
00382 }
00383
00384 void ompl::geometric::RRTstar::freeMemory(void)
00385 {
00386 if (nn_)
00387 {
00388 std::vector<Motion*> motions;
00389 nn_->list(motions);
00390 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00391 {
00392 if (motions[i]->state)
00393 si_->freeState(motions[i]->state);
00394 delete motions[i];
00395 }
00396 }
00397 }
00398
00399 void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
00400 {
00401 Planner::getPlannerData(data);
00402
00403 std::vector<Motion*> motions;
00404 if (nn_)
00405 nn_->list(motions);
00406
00407 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00408 data.addEdge (base::PlannerDataVertex (motions[i]->parent ? motions[i]->parent->state : NULL),
00409 base::PlannerDataVertex (motions[i]->state));
00410 }