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/BallTreeRRTstar.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <algorithm>
00042 #include <limits>
00043 #include <map>
00044
00045 ompl::geometric::BallTreeRRTstar::BallTreeRRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "BallTreeRRTstar")
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 rO_ = std::numeric_limits<double>::infinity();
00055 delayCC_ = true;
00056
00057 Planner::declareParam<double>("range", this, &BallTreeRRTstar::setRange, &BallTreeRRTstar::getRange);
00058 Planner::declareParam<double>("goal_bias", this, &BallTreeRRTstar::setGoalBias, &BallTreeRRTstar::getGoalBias);
00059 Planner::declareParam<double>("ball_radius_constant", this, &BallTreeRRTstar::setBallRadiusConstant, &BallTreeRRTstar::getBallRadiusConstant);
00060 Planner::declareParam<double>("max_ball_radius", this, &BallTreeRRTstar::setMaxBallRadius, &BallTreeRRTstar::getMaxBallRadius);
00061 Planner::declareParam<double>("initial_volume_radius", this, &BallTreeRRTstar::setInitialVolumeRadius, &BallTreeRRTstar::getInitialVolumeRadius);
00062 Planner::declareParam<bool>("delay_cc", this, &BallTreeRRTstar::setDelayCC, &BallTreeRRTstar::getDelayCC);
00063 }
00064
00065 ompl::geometric::BallTreeRRTstar::~BallTreeRRTstar(void)
00066 {
00067 freeMemory();
00068 }
00069
00070 void ompl::geometric::BallTreeRRTstar::setup(void)
00071 {
00072 Planner::setup();
00073 tools::SelfConfig sc(si_, getName());
00074 sc.configurePlannerRange(maxDistance_);
00075
00076 ballRadiusMax_ = si_->getMaximumExtent();
00077 ballRadiusConst_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
00078
00079 delayCC_ = true;
00080
00081 if (!nn_)
00082 nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
00083 nn_->setDistanceFunction(boost::bind(&BallTreeRRTstar::distanceFunction, this, _1, _2));
00084 }
00085
00086 void ompl::geometric::BallTreeRRTstar::clear(void)
00087 {
00088 Planner::clear();
00089 sampler_.reset();
00090 motions_.clear();
00091 freeMemory();
00092 if (nn_)
00093 nn_->clear();
00094 }
00095
00096 bool ompl::geometric::BallTreeRRTstar::solve(const base::PlannerTerminationCondition &ptc)
00097 {
00098 checkValidity();
00099 base::Goal *goal = pdef_->getGoal().get();
00100 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00101
00102 if (!goal)
00103 {
00104 msg_.error("Goal undefined");
00105 return false;
00106 }
00107
00108 while (const base::State *st = pis_.nextStart())
00109 {
00110 Motion *motion = new Motion(si_, rO_);
00111 si_->copyState(motion->state, st);
00112 addMotion(motion);
00113 }
00114
00115 if (nn_->size() == 0)
00116 {
00117 msg_.error("There are no valid initial states!");
00118 return false;
00119 }
00120
00121 if (!sampler_)
00122 sampler_ = si_->allocStateSampler();
00123
00124 msg_.inform("Starting with %u states", nn_->size());
00125
00126 Motion *solution = NULL;
00127 Motion *approximation = NULL;
00128 double approximatedist = std::numeric_limits<double>::infinity();
00129 bool sufficientlyShort = false;
00130
00131 Motion *rmotion = new Motion(si_, rO_);
00132 Motion *toTrim = NULL;
00133 base::State *rstate = rmotion->state;
00134 base::State *xstate = si_->allocState();
00135 base::State *tstate = si_->allocState();
00136 std::vector<Motion*> solCheck;
00137 std::vector<Motion*> nbh;
00138 std::vector<double> dists;
00139 std::vector<int> valid;
00140 long unsigned int rewireTest = 0;
00141
00142 std::pair<base::State*,double> lastValid(tstate, 0.0);
00143
00144 while (ptc() == false)
00145 {
00146 bool rejected = false;
00147
00148
00149 do
00150 {
00151
00152 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00153 goal_s->sampleGoal(rstate);
00154 else
00155 sampler_->sampleUniform(rstate);
00156
00157
00158 if (inVolume(rstate))
00159 {
00160 rejected = true;
00161
00162
00163 if(!si_->isValid(rstate))
00164 {
00165
00166
00167 toTrim = nn_->nearest(rmotion);
00168 double newRad = si_->distance(toTrim->state, rstate);
00169 if (newRad < toTrim->volRadius)
00170 toTrim->volRadius = newRad;
00171 }
00172
00173 }
00174 else
00175
00176 rejected = false;
00177
00178 }
00179 while (rejected);
00180
00181
00182 Motion *nmotion = nn_->nearest(rmotion);
00183
00184 base::State *dstate = rstate;
00185
00186
00187 double d = si_->distance(nmotion->state, rstate);
00188 if (d > maxDistance_)
00189 {
00190 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00191 dstate = xstate;
00192 }
00193
00194 if (si_->checkMotion(nmotion->state, dstate, lastValid))
00195 {
00196
00197 double distN = si_->distance(dstate, nmotion->state);
00198 Motion *motion = new Motion(si_, rO_);
00199 si_->copyState(motion->state, dstate);
00200 motion->parent = nmotion;
00201 motion->cost = nmotion->cost + distN;
00202
00203
00204 double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
00205 ballRadiusMax_);
00206
00207 nn_->nearestR(motion, r, nbh);
00208 rewireTest += nbh.size();
00209
00210
00211 dists.resize(nbh.size());
00212
00213 valid.resize(nbh.size());
00214 std::fill(valid.begin(), valid.end(), 0);
00215
00216 if (delayCC_)
00217 {
00218
00219 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00220 if (nbh[i] != nmotion)
00221 {
00222 double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
00223 nbh[i]->cost = c;
00224 }
00225
00226
00227 std::sort(nbh.begin(), nbh.end(), compareMotion);
00228
00229 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00230 if (nbh[i] != nmotion)
00231 {
00232 dists[i] = si_->distance(nbh[i]->state, dstate);
00233 nbh[i]->cost -= dists[i];
00234 }
00235
00236 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00237 if (nbh[i] != nmotion)
00238 {
00239
00240 dists[i] = si_->distance(nbh[i]->state, dstate);
00241 double c = nbh[i]->cost + dists[i];
00242 if (c < motion->cost)
00243 {
00244 if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
00245 {
00246 motion->cost = c;
00247 motion->parent = nbh[i];
00248 valid[i] = 1;
00249 break;
00250 }
00251 else
00252 {
00253 valid[i] = -1;
00254
00255 double nR = si_->distance(nbh[i]->state, lastValid.first);
00256 if (nR < nbh[i]->volRadius)
00257 nbh[i]->volRadius = nR;
00258 }
00259 }
00260 }
00261 else
00262 {
00263 valid[i] = 1;
00264 dists[i] = distN;
00265 break;
00266 }
00267
00268 }
00269 else{
00270
00271 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00272 if (nbh[i] != nmotion)
00273 {
00274
00275 dists[i] = si_->distance(nbh[i]->state, dstate);
00276 double c = nbh[i]->cost + dists[i];
00277 if (c < motion->cost)
00278 {
00279 if (si_->checkMotion(nbh[i]->state, dstate, lastValid))
00280 {
00281 motion->cost = c;
00282 motion->parent = nbh[i];
00283 valid[i] = 1;
00284 }
00285 else
00286 {
00287 valid[i] = -1;
00288
00289 double newR = si_->distance(nbh[i]->state, lastValid.first);
00290 if (newR < nbh[i]->volRadius)
00291 nbh[i]->volRadius = newR;
00292
00293 }
00294 }
00295 }
00296 else
00297 {
00298 valid[i] = 1;
00299 dists[i] = distN;
00300 }
00301 }
00302
00303
00304 addMotion(motion);
00305 motion->parent->children.push_back(motion);
00306
00307 solCheck.resize(1);
00308 solCheck[0] = motion;
00309
00310
00311 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00312 if (nbh[i] != motion->parent)
00313 {
00314 double c = motion->cost + dists[i];
00315 if (c < nbh[i]->cost)
00316 {
00317 bool v = false;
00318 if (valid[i] == 0)
00319 {
00320 if(!si_->checkMotion(nbh[i]->state, dstate, lastValid))
00321 {
00322
00323 double R = si_->distance(nbh[i]->state, lastValid.first);
00324 if (R < nbh[i]->volRadius)
00325 nbh[i]->volRadius = R;
00326 }
00327 else
00328 {
00329 v = true;
00330 }
00331 }
00332 if (valid[i] == 1)
00333 v = true;
00334
00335 if (v)
00336 {
00337
00338 removeFromParent (nbh[i]);
00339 double delta = c - nbh[i]->cost;
00340
00341 nbh[i]->parent = motion;
00342 nbh[i]->cost = c;
00343 nbh[i]->parent->children.push_back(nbh[i]);
00344 solCheck.push_back(nbh[i]);
00345
00346
00347 updateChildCosts(nbh[i], delta);
00348 }
00349 }
00350 }
00351
00352
00353 for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00354 {
00355 double dist = 0.0;
00356 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00357 sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00358
00359 if (solved)
00360 {
00361 if (sufficientlyShort)
00362 {
00363 solution = solCheck[i];
00364 break;
00365 }
00366 else if (!solution || (solCheck[i]->cost < solution->cost))
00367 {
00368 solution = solCheck[i];
00369 }
00370 }
00371 else if (!solution && dist < approximatedist)
00372 {
00373 approximation = solCheck[i];
00374 approximatedist = dist;
00375 }
00376 }
00377
00378
00379 if (solution && sufficientlyShort)
00380 break;
00381 }
00382 else
00383 {
00384
00385 toTrim = nn_->nearest(nmotion);
00386 double newRadius = si_->distance(toTrim->state, lastValid.first);
00387 if (newRadius < toTrim->volRadius)
00388 toTrim->volRadius = newRadius;
00389 }
00390 }
00391
00392 double solutionCost;
00393 bool approximate = (solution == NULL);
00394 bool addedSolution = false;
00395 if (approximate)
00396 {
00397 solution = approximation;
00398 solutionCost = approximatedist;
00399 }
00400 else
00401 solutionCost = solution->cost;
00402
00403 if (solution != NULL)
00404 {
00405
00406 std::vector<Motion*> mpath;
00407 while (solution != NULL)
00408 {
00409 mpath.push_back(solution);
00410 solution = solution->parent;
00411 }
00412
00413
00414 PathGeometric *path = new PathGeometric(si_);
00415 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00416 path->append(mpath[i]->state);
00417 goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
00418 addedSolution = true;
00419 }
00420
00421 si_->freeState(xstate);
00422 if (rmotion->state)
00423 si_->freeState(rmotion->state);
00424 delete rmotion;
00425
00426 msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00427
00428 return addedSolution;
00429 }
00430
00431 void ompl::geometric::BallTreeRRTstar::removeFromParent(Motion *m)
00432 {
00433 std::vector<Motion*>::iterator it = m->parent->children.begin ();
00434 while (it != m->parent->children.end ())
00435 {
00436 if (*it == m)
00437 {
00438 it = m->parent->children.erase(it);
00439 it = m->parent->children.end ();
00440 }
00441 else
00442 ++it;
00443 }
00444 }
00445
00446 void ompl::geometric::BallTreeRRTstar::updateChildCosts(Motion *m, double delta)
00447 {
00448 for (size_t i = 0; i < m->children.size(); ++i)
00449 {
00450 m->children[i]->cost += delta;
00451 updateChildCosts(m->children[i], delta);
00452 }
00453 }
00454
00455 void ompl::geometric::BallTreeRRTstar::freeMemory(void)
00456 {
00457 if (nn_)
00458 {
00459 std::vector<Motion*> motions;
00460 nn_->list(motions);
00461 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00462 {
00463 if (motions[i]->state)
00464 si_->freeState(motions[i]->state);
00465 delete motions[i];
00466 }
00467 }
00468 }
00469
00470 void ompl::geometric::BallTreeRRTstar::getPlannerData(base::PlannerData &data) const
00471 {
00472 Planner::getPlannerData(data);
00473
00474 std::vector<Motion*> motions;
00475 if (nn_)
00476 nn_->list(motions);
00477
00478 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00479 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00480 }