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/kpiece/LBKPIECE1.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include <cassert>
00040
00041 void ompl::geometric::LBKPIECE1::setup(void)
00042 {
00043 Planner::setup();
00044 checkProjectionEvaluator(this, projectionEvaluator_);
00045 checkMotionLength(this, maxDistance_);
00046
00047 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00048 throw Exception("The minimum valid path fraction must be in the range (0,1]");
00049
00050 dStart_.setDimension(projectionEvaluator_->getDimension());
00051 dGoal_.setDimension(projectionEvaluator_->getDimension());
00052 }
00053
00054 bool ompl::geometric::LBKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00055 {
00056 checkValidity();
00057 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00058
00059 if (!goal)
00060 {
00061 msg_.error("Unknown type of goal (or goal undefined)");
00062 return false;
00063 }
00064
00065 Discretization<Motion>::Coord xcoord;
00066
00067 while (const base::State *st = pis_.nextStart())
00068 {
00069 Motion* motion = new Motion(si_);
00070 si_->copyState(motion->state, st);
00071 motion->root = st;
00072 motion->valid = true;
00073 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00074 dStart_.addMotion(motion, xcoord);
00075 }
00076
00077 if (dStart_.getMotionCount() == 0)
00078 {
00079 msg_.error("Motion planning start tree could not be initialized!");
00080 return false;
00081 }
00082
00083 if (!goal->canSample())
00084 {
00085 msg_.error("Insufficient states in sampleable goal region");
00086 return false;
00087 }
00088
00089 if (!sampler_)
00090 sampler_ = si_->allocManifoldStateSampler();
00091
00092 msg_.inform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
00093
00094 base::State *xstate = si_->allocState();
00095 bool startTree = true;
00096
00097 while (ptc() == false)
00098 {
00099 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
00100 startTree = !startTree;
00101 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
00102 disc.countIteration();
00103
00104
00105 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
00106 {
00107 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00108 if (st)
00109 {
00110 Motion* motion = new Motion(si_);
00111 si_->copyState(motion->state, st);
00112 motion->root = motion->state;
00113 motion->valid = true;
00114 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00115 dGoal_.addMotion(motion, xcoord);
00116 }
00117 if (dGoal_.getMotionCount() == 0)
00118 {
00119 msg_.error("Unable to sample any valid states for goal tree");
00120 break;
00121 }
00122 }
00123
00124 Discretization<Motion>::Cell *ecell = NULL;
00125 Motion *existing = NULL;
00126 disc.selectMotion(existing, ecell);
00127 assert(existing);
00128 sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
00129
00130
00131 Motion* motion = new Motion(si_);
00132 si_->copyState(motion->state, xstate);
00133 motion->parent = existing;
00134 motion->root = existing->root;
00135 existing->children.push_back(motion);
00136 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00137 disc.addMotion(motion, xcoord);
00138
00139
00140 Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
00141 if (ocell && !ocell->data->motions.empty())
00142 {
00143 Motion* connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
00144
00145 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
00146 {
00147 Motion* connect = new Motion(si_);
00148 si_->copyState(connect->state, connectOther->state);
00149 connect->parent = motion;
00150 connect->root = motion->root;
00151 motion->children.push_back(connect);
00152 projectionEvaluator_->computeCoordinates(connect->state, xcoord);
00153 disc.addMotion(connect, xcoord);
00154
00155 if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
00156 {
00157
00158
00159 std::vector<Motion*> mpath1;
00160 while (motion != NULL)
00161 {
00162 mpath1.push_back(motion);
00163 motion = motion->parent;
00164 }
00165
00166 std::vector<Motion*> mpath2;
00167 while (connectOther != NULL)
00168 {
00169 mpath2.push_back(connectOther);
00170 connectOther = connectOther->parent;
00171 }
00172
00173 if (startTree)
00174 mpath1.swap(mpath2);
00175
00176 PathGeometric *path = new PathGeometric(si_);
00177 path->states.reserve(mpath1.size() + mpath2.size());
00178 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00179 path->states.push_back(si_->cloneState(mpath1[i]->state));
00180 for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00181 path->states.push_back(si_->cloneState(mpath2[i]->state));
00182
00183 goal->setDifference(0.0);
00184 goal->setSolutionPath(base::PathPtr(path));
00185 break;
00186 }
00187 }
00188 }
00189 }
00190
00191 si_->freeState(xstate);
00192
00193 msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
00194 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
00195 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
00196 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
00197
00198 return goal->isAchieved();
00199 }
00200
00201 bool ompl::geometric::LBKPIECE1::isPathValid(Discretization<Motion> &disc, Motion *motion, base::State *temp)
00202 {
00203 std::vector<Motion*> mpath;
00204
00205
00206 while (motion != NULL)
00207 {
00208 mpath.push_back(motion);
00209 motion = motion->parent;
00210 }
00211
00212 std::pair<base::State*, double> lastValid;
00213 lastValid.first = temp;
00214
00215
00216 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00217 if (!mpath[i]->valid)
00218 {
00219 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
00220 mpath[i]->valid = true;
00221 else
00222 {
00223 Motion *parent = mpath[i]->parent;
00224 removeMotion(disc, mpath[i]);
00225
00226
00227 if (lastValid.second > minValidPathFraction_)
00228 {
00229 Motion* reAdd = new Motion(si_);
00230 si_->copyState(reAdd->state, lastValid.first);
00231 reAdd->parent = parent;
00232 reAdd->root = parent->root;
00233 parent->children.push_back(reAdd);
00234 reAdd->valid = true;
00235 Discretization<Motion>::Coord coord;
00236 projectionEvaluator_->computeCoordinates(reAdd->state, coord);
00237 disc.addMotion(reAdd, coord);
00238 }
00239
00240 return false;
00241 }
00242 }
00243 return true;
00244 }
00245
00246 void ompl::geometric::LBKPIECE1::removeMotion(Discretization<Motion> &disc, Motion *motion)
00247 {
00248
00249
00250 Discretization<Motion>::Coord coord;
00251 projectionEvaluator_->computeCoordinates(motion->state, coord);
00252 disc.removeMotion(motion, coord);
00253
00254
00255
00256 if (motion->parent)
00257 {
00258 for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00259 if (motion->parent->children[i] == motion)
00260 {
00261 motion->parent->children.erase(motion->parent->children.begin() + i);
00262 break;
00263 }
00264 }
00265
00266
00267 for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00268 {
00269 motion->children[i]->parent = NULL;
00270 removeMotion(disc, motion->children[i]);
00271 }
00272
00273 freeMotion(motion);
00274 }
00275
00276
00277 void ompl::geometric::LBKPIECE1::freeMotion(Motion *motion)
00278 {
00279 if (motion->state)
00280 si_->freeState(motion->state);
00281 delete motion;
00282 }
00283
00284 void ompl::geometric::LBKPIECE1::clear(void)
00285 {
00286 Planner::clear();
00287
00288 sampler_.reset();
00289 dStart_.clear();
00290 dGoal_.clear();
00291 }
00292
00293 void ompl::geometric::LBKPIECE1::getPlannerData(base::PlannerData &data) const
00294 {
00295 Planner::getPlannerData(data);
00296 dStart_.getPlannerData(data, 1);
00297 dGoal_.getPlannerData(data, 2);
00298 }