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