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/geometric/planners/rrt/RRTConnect.h"
00038 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041
00042 ompl::geometric::RRTConnect::RRTConnect(const base::SpaceInformationPtr &si) : base::Planner(si, "RRTConnect")
00043 {
00044 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00045 maxDistance_ = 0.0;
00046
00047 Planner::declareParam<double>("range", this, &RRTConnect::setRange, &RRTConnect::getRange);
00048 }
00049
00050 ompl::geometric::RRTConnect::~RRTConnect(void)
00051 {
00052 freeMemory();
00053 }
00054
00055 void ompl::geometric::RRTConnect::setup(void)
00056 {
00057 Planner::setup();
00058 tools::SelfConfig sc(si_, getName());
00059 sc.configurePlannerRange(maxDistance_);
00060
00061 if (!tStart_)
00062 tStart_.reset(new NearestNeighborsGNAT<Motion*>());
00063 if (!tGoal_)
00064 tGoal_.reset(new NearestNeighborsGNAT<Motion*>());
00065 tStart_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
00066 tGoal_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
00067 }
00068
00069 void ompl::geometric::RRTConnect::freeMemory(void)
00070 {
00071 std::vector<Motion*> motions;
00072
00073 if (tStart_)
00074 {
00075 tStart_->list(motions);
00076 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00077 {
00078 if (motions[i]->state)
00079 si_->freeState(motions[i]->state);
00080 delete motions[i];
00081 }
00082 }
00083
00084 if (tGoal_)
00085 {
00086 tGoal_->list(motions);
00087 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00088 {
00089 if (motions[i]->state)
00090 si_->freeState(motions[i]->state);
00091 delete motions[i];
00092 }
00093 }
00094 }
00095
00096 void ompl::geometric::RRTConnect::clear(void)
00097 {
00098 Planner::clear();
00099 sampler_.reset();
00100 freeMemory();
00101 if (tStart_)
00102 tStart_->clear();
00103 if (tGoal_)
00104 tGoal_->clear();
00105 }
00106
00107 ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
00108 {
00109
00110 Motion *nmotion = tree->nearest(rmotion);
00111
00112
00113 bool reach = true;
00114
00115
00116 base::State *dstate = rmotion->state;
00117 double d = si_->distance(nmotion->state, rmotion->state);
00118 if (d > maxDistance_)
00119 {
00120 si_->getStateSpace()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
00121 dstate = tgi.xstate;
00122 reach = false;
00123 }
00124
00125 if (si_->checkMotion(nmotion->state, dstate))
00126 {
00127
00128 Motion *motion = new Motion(si_);
00129 si_->copyState(motion->state, dstate);
00130 motion->parent = nmotion;
00131 motion->root = nmotion->root;
00132 tgi.xmotion = motion;
00133
00134 tree->add(motion);
00135 if (reach)
00136 return REACHED;
00137 else
00138 return ADVANCED;
00139 }
00140 else
00141 return TRAPPED;
00142 }
00143
00144 bool ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
00145 {
00146 checkValidity();
00147 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00148
00149 if (!goal)
00150 {
00151 msg_.error("Unknown type of goal (or goal undefined)");
00152 return false;
00153 }
00154
00155 while (const base::State *st = pis_.nextStart())
00156 {
00157 Motion *motion = new Motion(si_);
00158 si_->copyState(motion->state, st);
00159 motion->root = motion->state;
00160 tStart_->add(motion);
00161 }
00162
00163 if (tStart_->size() == 0)
00164 {
00165 msg_.error("Motion planning start tree could not be initialized!");
00166 return false;
00167 }
00168
00169 if (!goal->couldSample())
00170 {
00171 msg_.error("Insufficient states in sampleable goal region");
00172 return false;
00173 }
00174
00175 if (!sampler_)
00176 sampler_ = si_->allocStateSampler();
00177
00178 msg_.inform("Starting with %d states", (int)(tStart_->size() + tGoal_->size()));
00179
00180 TreeGrowingInfo tgi;
00181 tgi.xstate = si_->allocState();
00182
00183 Motion *rmotion = new Motion(si_);
00184 base::State *rstate = rmotion->state;
00185 bool startTree = true;
00186 bool solved = false;
00187
00188 while (ptc() == false)
00189 {
00190 TreeData &tree = startTree ? tStart_ : tGoal_;
00191 startTree = !startTree;
00192 TreeData &otherTree = startTree ? tStart_ : tGoal_;
00193
00194 if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
00195 {
00196 const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00197 if (st)
00198 {
00199 Motion* motion = new Motion(si_);
00200 si_->copyState(motion->state, st);
00201 motion->root = motion->state;
00202 tGoal_->add(motion);
00203 }
00204
00205 if (tGoal_->size() == 0)
00206 {
00207 msg_.error("Unable to sample any valid states for goal tree");
00208 break;
00209 }
00210 }
00211
00212
00213 sampler_->sampleUniform(rstate);
00214
00215 GrowState gs = growTree(tree, tgi, rmotion);
00216
00217 if (gs != TRAPPED)
00218 {
00219
00220 Motion *addedMotion = tgi.xmotion;
00221
00222
00223
00224
00225 if (gs != REACHED)
00226 si_->copyState(rstate, tgi.xstate);
00227
00228 GrowState gsc = ADVANCED;
00229 while (gsc == ADVANCED)
00230 gsc = growTree(otherTree, tgi, rmotion);
00231
00232
00233 if (gsc == REACHED && goal->isStartGoalPairValid(startTree ? tgi.xmotion->root : addedMotion->root,
00234 startTree ? addedMotion->root : tgi.xmotion->root))
00235 {
00236
00237 Motion *solution = tgi.xmotion;
00238 std::vector<Motion*> mpath1;
00239 while (solution != NULL)
00240 {
00241 mpath1.push_back(solution);
00242 solution = solution->parent;
00243 }
00244
00245 solution = addedMotion;
00246 std::vector<Motion*> mpath2;
00247 while (solution != NULL)
00248 {
00249 mpath2.push_back(solution);
00250 solution = solution->parent;
00251 }
00252
00253 if (!startTree)
00254 mpath2.swap(mpath1);
00255
00256 PathGeometric *path = new PathGeometric(si_);
00257 path->getStates().reserve(mpath1.size() + mpath2.size());
00258 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00259 path->append(mpath1[i]->state);
00260 for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00261 path->append(mpath2[i]->state);
00262
00263 goal->addSolutionPath(base::PathPtr(path), false, 0.0);
00264 solved = true;
00265 break;
00266 }
00267 }
00268 }
00269
00270 si_->freeState(tgi.xstate);
00271 si_->freeState(rstate);
00272 delete rmotion;
00273
00274 msg_.inform("Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());
00275
00276 return solved;
00277 }
00278
00279 void ompl::geometric::RRTConnect::getPlannerData(base::PlannerData &data) const
00280 {
00281 Planner::getPlannerData(data);
00282
00283 std::vector<Motion*> motions;
00284 if (tStart_)
00285 tStart_->list(motions);
00286
00287 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00288 {
00289 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00290 data.tagState(motions[i]->state, 1);
00291 }
00292
00293 motions.clear();
00294 if (tGoal_)
00295 tGoal_->list(motions);
00296
00297 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00298 {
00299 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00300 data.tagState(motions[i]->state, 2);
00301 }
00302 }