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 void ompl::geometric::RRTstar::setup(void)
00046 {
00047 Planner::setup();
00048 SelfConfig sc(si_, getName());
00049 sc.configurePlannerRange(maxDistance_);
00050
00051 ballRadiusMax_ = si_->getMaximumExtent();
00052 ballRadiusConst_ = maxDistance_ * sqrt(si_->getStateSpace()->getDimension());
00053
00054 delayCC_ = true;
00055
00056 if (!nn_)
00057 nn_.reset(new NearestNeighborsGNAT<Motion*>());
00058 nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
00059 }
00060
00061 void ompl::geometric::RRTstar::clear(void)
00062 {
00063 Planner::clear();
00064 sampler_.reset();
00065 freeMemory();
00066 if (nn_)
00067 nn_->clear();
00068 }
00069
00070 bool ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
00071 {
00072 checkValidity();
00073 base::Goal *goal = pdef_->getGoal().get();
00074 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00075
00076 if (!goal)
00077 {
00078 msg_.error("Goal undefined");
00079 return false; }
00080 while (const base::State *st = pis_.nextStart())
00081 {
00082 Motion *motion = new Motion(si_);
00083 si_->copyState(motion->state, st);
00084 nn_->add(motion);
00085 }
00086
00087 if (nn_->size() == 0)
00088 {
00089 msg_.error("There are no valid initial states!");
00090 return false;
00091 }
00092
00093 if (!sampler_)
00094 sampler_ = si_->allocStateSampler();
00095
00096 msg_.inform("Starting with %u states", nn_->size());
00097
00098 Motion *solution = NULL;
00099 Motion *approxsol = NULL;
00100 double approxdif = std::numeric_limits<double>::infinity();
00101 bool approxsolved = false;
00102
00103 Motion *rmotion = new Motion(si_);
00104 base::State *rstate = rmotion->state;
00105 base::State *xstate = si_->allocState();
00106 std::vector<Motion*> solCheck;
00107 std::vector<Motion*> nbh;
00108 std::vector<double> dists;
00109 std::vector<int> valid;
00110 long unsigned int rewireTest = 0;
00111
00112 while (ptc() == false)
00113 {
00114
00115 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00116 goal_s->sampleGoal(rstate);
00117 else
00118 sampler_->sampleUniform(rstate);
00119
00120
00121 Motion *nmotion = nn_->nearest(rmotion);
00122
00123 base::State *dstate = rstate;
00124
00125
00126 double d = si_->distance(nmotion->state, rstate);
00127 if (d > maxDistance_)
00128 {
00129 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00130 dstate = xstate;
00131 }
00132
00133 if (si_->checkMotion(nmotion->state, dstate))
00134 {
00135
00136 double distN = si_->distance(dstate, nmotion->state);
00137 Motion *motion = new Motion(si_);
00138 si_->copyState(motion->state, dstate);
00139 motion->parent = nmotion;
00140 motion->cost = nmotion->cost + distN;
00141
00142
00143 double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
00144 ballRadiusMax_);
00145
00146 nn_->nearestR(motion, r, nbh);
00147 rewireTest += nbh.size();
00148
00149
00150 dists.resize(nbh.size());
00151
00152 valid.resize(nbh.size());
00153 std::fill(valid.begin(), valid.end(), 0);
00154
00155 if(delayCC_)
00156 {
00157
00158 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00159 if (nbh[i] != nmotion)
00160 {
00161 double c = nbh[i]->cost + si_->distance(nbh[i]->state, dstate);
00162 nbh[i]->cost = c;
00163 }
00164
00165
00166 std::sort(nbh.begin(), nbh.end(), compareMotion);
00167
00168 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00169 if (nbh[i] != nmotion)
00170 {
00171 dists[i] = si_->distance(nbh[i]->state, dstate);
00172 nbh[i]->cost -= dists[i];
00173 }
00174
00175
00176 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00177 if (nbh[i] != nmotion)
00178 {
00179
00180 dists[i] = si_->distance(nbh[i]->state, dstate);
00181 double c = nbh[i]->cost + dists[i];
00182 if (c < motion->cost)
00183 {
00184 if (si_->checkMotion(nbh[i]->state, dstate))
00185 {
00186 motion->cost = c;
00187 motion->parent = nbh[i];
00188 valid[i] = 1;
00189 break;
00190 }
00191 else
00192 valid[i] = -1;
00193 }
00194 }
00195 else
00196 {
00197 valid[i] = 1;
00198 dists[i] = distN;
00199 break;
00200 }
00201
00202 }
00203 else
00204 {
00205
00206 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00207 if (nbh[i] != nmotion)
00208 {
00209
00210 dists[i] = si_->distance(nbh[i]->state, dstate);
00211 double c = nbh[i]->cost + dists[i];
00212 if (c < motion->cost)
00213 {
00214 if (si_->checkMotion(nbh[i]->state, dstate))
00215 {
00216 motion->cost = c;
00217 motion->parent = nbh[i];
00218 valid[i] = 1;
00219 }
00220 else
00221 valid[i] = -1;
00222 }
00223 }
00224 else
00225 {
00226 valid[i] = 1;
00227 dists[i] = distN;
00228 }
00229
00230 }
00231
00232
00233 nn_->add(motion);
00234
00235 solCheck.resize(1);
00236 solCheck[0] = motion;
00237
00238
00239 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00240 if (nbh[i] != motion->parent)
00241 {
00242 double c = motion->cost + dists[i];
00243 if (c < nbh[i]->cost)
00244 {
00245 bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
00246 if (v)
00247 {
00248 nbh[i]->parent = motion;
00249 nbh[i]->cost = c;
00250 solCheck.push_back(nbh[i]);
00251 }
00252 }
00253 }
00254
00255
00256 for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
00257 {
00258 double dist = 0.0;
00259 bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
00260 bool sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
00261
00262 if (solved)
00263 {
00264 if (sufficientlyShort)
00265 {
00266 solution = solCheck[i];
00267 break;
00268 }
00269 else
00270 {
00271 if (approxsolved)
00272 {
00273 if (dist < approxdif)
00274 {
00275 approxdif = dist;
00276 approxsol = solCheck[i];
00277 }
00278 }
00279 else
00280 {
00281 approxsolved = true;
00282 approxdif = dist;
00283 approxsol = solCheck[i];
00284 }
00285 }
00286 }
00287 else
00288 if (!approxsolved && dist < approxdif)
00289 {
00290 approxdif = dist;
00291 approxsol = solCheck[i];
00292 }
00293 }
00294
00295
00296 if (solution != NULL)
00297 break;
00298 }
00299 }
00300
00301 bool approximate = false;
00302 if (solution == NULL)
00303 {
00304 solution = approxsol;
00305 approximate = true;
00306 }
00307
00308 if (solution != NULL)
00309 {
00310
00311 std::vector<Motion*> mpath;
00312 while (solution != NULL)
00313 {
00314 mpath.push_back(solution);
00315 solution = solution->parent;
00316 }
00317
00318
00319 PathGeometric *path = new PathGeometric(si_);
00320 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00321 path->states.push_back(si_->cloneState(mpath[i]->state));
00322 goal->setDifference(approxdif);
00323 goal->setSolutionPath(base::PathPtr(path), approximate);
00324
00325 if (approximate)
00326 msg_.warn("Found approximate solution");
00327 }
00328
00329 si_->freeState(xstate);
00330 if (rmotion->state)
00331 si_->freeState(rmotion->state);
00332 delete rmotion;
00333
00334 msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
00335
00336 return goal->isAchieved();
00337 }
00338
00339 void ompl::geometric::RRTstar::freeMemory(void)
00340 {
00341 if (nn_)
00342 {
00343 std::vector<Motion*> motions;
00344 nn_->list(motions);
00345 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00346 {
00347 if (motions[i]->state)
00348 si_->freeState(motions[i]->state);
00349 delete motions[i];
00350 }
00351 }
00352 }
00353
00354 void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
00355 {
00356 Planner::getPlannerData(data);
00357
00358 std::vector<Motion*> motions;
00359 if (nn_)
00360 nn_->list(motions);
00361
00362 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00363 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00364 }