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