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