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