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