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