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/BKPIECE1.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <cassert>
00041
00042 ompl::geometric::BKPIECE1::BKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "BKPIECE1"),
00043 dStart_(boost::bind(&BKPIECE1::freeMotion, this, _1)),
00044 dGoal_(boost::bind(&BKPIECE1::freeMotion, this, _1))
00045 {
00046 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00047
00048 minValidPathFraction_ = 0.5;
00049 failedExpansionScoreFactor_ = 0.5;
00050 maxDistance_ = 0.0;
00051 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
00052
00053 Planner::declareParam<double>("range", this, &BKPIECE1::setRange, &BKPIECE1::getRange);
00054 Planner::declareParam<double>("border_fraction", this, &BKPIECE1::setBorderFraction, &BKPIECE1::getBorderFraction);
00055 Planner::declareParam<double>("failed_expansion_score_factor", this, &BKPIECE1::setFailedExpansionCellScoreFactor, &BKPIECE1::getFailedExpansionCellScoreFactor);
00056 Planner::declareParam<double>("min_valid_path_fraction", this, &BKPIECE1::setMinValidPathFraction, &BKPIECE1::getMinValidPathFraction);
00057 }
00058
00059 ompl::geometric::BKPIECE1::~BKPIECE1(void)
00060 {
00061 }
00062
00063 void ompl::geometric::BKPIECE1::setup(void)
00064 {
00065 Planner::setup();
00066 tools::SelfConfig sc(si_, getName());
00067 sc.configureProjectionEvaluator(projectionEvaluator_);
00068 sc.configurePlannerRange(maxDistance_);
00069
00070 if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
00071 throw Exception("Failed expansion cell score factor must be in the range (0,1]");
00072 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00073 throw Exception("The minimum valid path fraction must be in the range (0,1]");
00074
00075 dStart_.setDimension(projectionEvaluator_->getDimension());
00076 dGoal_.setDimension(projectionEvaluator_->getDimension());
00077 }
00078
00079 ompl::base::PlannerStatus ompl::geometric::BKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00080 {
00081 checkValidity();
00082 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00083
00084 if (!goal)
00085 {
00086 logError("Unknown type of goal (or goal undefined)");
00087 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
00088 }
00089
00090 Discretization<Motion>::Coord xcoord;
00091
00092 while (const base::State *st = pis_.nextStart())
00093 {
00094 Motion* motion = new Motion(si_);
00095 si_->copyState(motion->state, st);
00096 motion->root = motion->state;
00097 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00098 dStart_.addMotion(motion, xcoord);
00099 }
00100
00101 if (dStart_.getMotionCount() == 0)
00102 {
00103 logError("Motion planning start tree could not be initialized!");
00104 return base::PlannerStatus::INVALID_START;
00105 }
00106
00107 if (!goal->couldSample())
00108 {
00109 logError("Insufficient states in sampleable goal region");
00110 return base::PlannerStatus::INVALID_GOAL;
00111 }
00112
00113 if (!sampler_)
00114 sampler_ = si_->allocValidStateSampler();
00115
00116 logInform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
00117
00118 std::vector<Motion*> solution;
00119 base::State *xstate = si_->allocState();
00120 bool startTree = true;
00121 bool solved = false;
00122
00123 while (ptc() == false)
00124 {
00125 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
00126 startTree = !startTree;
00127 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
00128 disc.countIteration();
00129
00130
00131 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
00132 {
00133 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00134 if (st)
00135 {
00136 Motion* motion = new Motion(si_);
00137 si_->copyState(motion->state, st);
00138 motion->root = motion->state;
00139 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00140 dGoal_.addMotion(motion, xcoord);
00141 }
00142 if (dGoal_.getMotionCount() == 0)
00143 {
00144 logError("Unable to sample any valid states for goal tree");
00145 break;
00146 }
00147 }
00148
00149 Discretization<Motion>::Cell *ecell = NULL;
00150 Motion *existing = NULL;
00151 disc.selectMotion(existing, ecell);
00152 assert(existing);
00153 if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
00154 {
00155 std::pair<base::State*, double> fail(xstate, 0.0);
00156 bool keep = si_->checkMotion(existing->state, xstate, fail);
00157 if (!keep && fail.second > minValidPathFraction_)
00158 keep = true;
00159
00160 if (keep)
00161 {
00162
00163 Motion *motion = new Motion(si_);
00164 si_->copyState(motion->state, xstate);
00165 motion->root = existing->root;
00166 motion->parent = existing;
00167
00168 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00169 disc.addMotion(motion, xcoord);
00170
00171 Discretization<Motion>::Cell* cellC = otherDisc.getGrid().getCell(xcoord);
00172
00173 if (cellC && !cellC->data->motions.empty())
00174 {
00175 Motion* connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
00176
00177 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root) &&
00178 si_->checkMotion(motion->state, connectOther->state))
00179 {
00180 if (startTree)
00181 connectionPoint_ = std::make_pair<base::State*, base::State*>(connectOther->state, motion->state);
00182 else
00183 connectionPoint_ = std::make_pair<base::State*, base::State*>(motion->state, connectOther->state);
00184
00185
00186
00187 std::vector<Motion*> mpath1;
00188 while (motion != NULL)
00189 {
00190 mpath1.push_back(motion);
00191 motion = motion->parent;
00192 }
00193
00194 std::vector<Motion*> mpath2;
00195 while (connectOther != NULL)
00196 {
00197 mpath2.push_back(connectOther);
00198 connectOther = connectOther->parent;
00199 }
00200
00201 if (startTree)
00202 mpath1.swap(mpath2);
00203
00204 PathGeometric *path = new PathGeometric(si_);
00205 path->getStates().reserve(mpath1.size() + mpath2.size());
00206 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00207 path->append(mpath1[i]->state);
00208 for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00209 path->append(mpath2[i]->state);
00210
00211 pdef_->addSolutionPath(base::PathPtr(path), false, 0.0);
00212 solved = true;
00213 break;
00214 }
00215 }
00216 }
00217 else
00218 ecell->data->score *= failedExpansionScoreFactor_;
00219 }
00220 else
00221 ecell->data->score *= failedExpansionScoreFactor_;
00222 disc.updateCell(ecell);
00223 }
00224
00225 si_->freeState(xstate);
00226
00227 logInform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
00228 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
00229 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
00230 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
00231
00232 return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00233 }
00234
00235 void ompl::geometric::BKPIECE1::freeMotion(Motion *motion)
00236 {
00237 if (motion->state)
00238 si_->freeState(motion->state);
00239 delete motion;
00240 }
00241
00242 void ompl::geometric::BKPIECE1::clear(void)
00243 {
00244 Planner::clear();
00245
00246 sampler_.reset();
00247 dStart_.clear();
00248 dGoal_.clear();
00249 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
00250 }
00251
00252 void ompl::geometric::BKPIECE1::getPlannerData(base::PlannerData &data) const
00253 {
00254 Planner::getPlannerData(data);
00255 dStart_.getPlannerData(data, 1, true, NULL);
00256 dGoal_.getPlannerData(data, 2, false, NULL);
00257
00258
00259 data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
00260 }