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/GoalSampleableRegion.h"
00039 #include <cassert>
00040
00041 void ompl::geometric::BKPIECE1::setup(void)
00042 {
00043 Planner::setup();
00044 checkProjectionEvaluator(this, projectionEvaluator_);
00045 checkMotionLength(this, maxDistance_);
00046
00047 if (badScoreFactor_ < std::numeric_limits<double>::epsilon() || badScoreFactor_ > 1.0)
00048 throw Exception("Bad cell score factor must be in the range (0,1]");
00049 if (goodScoreFactor_ < std::numeric_limits<double>::epsilon() || goodScoreFactor_ > 1.0)
00050 throw Exception("Good cell score factor must be in the range (0,1]");
00051 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00052 throw Exception("The minimum valid path fraction must be in the range (0,1]");
00053
00054 dStart_.setDimension(projectionEvaluator_->getDimension());
00055 dGoal_.setDimension(projectionEvaluator_->getDimension());
00056 }
00057
00058 bool ompl::geometric::BKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00059 {
00060 checkValidity();
00061 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00062
00063 if (!goal)
00064 {
00065 msg_.error("Unknown type of goal (or goal undefined)");
00066 return false;
00067 }
00068
00069 Discretization<Motion>::Coord xcoord;
00070
00071 while (const base::State *st = pis_.nextStart())
00072 {
00073 Motion* motion = new Motion(si_);
00074 si_->copyState(motion->state, st);
00075 motion->root = motion->state;
00076 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00077 dStart_.addMotion(motion, xcoord);
00078 }
00079
00080 if (dStart_.getMotionCount() == 0)
00081 {
00082 msg_.error("Motion planning start tree could not be initialized!");
00083 return false;
00084 }
00085
00086 if (!goal->canSample())
00087 {
00088 msg_.error("Insufficient states in sampleable goal region");
00089 return false;
00090 }
00091
00092 if (!sampler_)
00093 sampler_ = si_->allocValidStateSampler();
00094
00095 msg_.inform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
00096
00097 std::vector<Motion*> solution;
00098 base::State *xstate = si_->allocState();
00099 bool startTree = true;
00100
00101 while (ptc() == false)
00102 {
00103 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
00104 startTree = !startTree;
00105 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
00106 disc.countIteration();
00107
00108
00109 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
00110 {
00111 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00112 if (st)
00113 {
00114 Motion* motion = new Motion(si_);
00115 si_->copyState(motion->state, st);
00116 motion->root = motion->state;
00117 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00118 dGoal_.addMotion(motion, xcoord);
00119 }
00120 if (dGoal_.getMotionCount() == 0)
00121 {
00122 msg_.error("Unable to sample any valid states for goal tree");
00123 break;
00124 }
00125 }
00126
00127 Discretization<Motion>::Cell *ecell = NULL;
00128 Motion *existing = NULL;
00129 disc.selectMotion(existing, ecell);
00130 assert(existing);
00131 if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
00132 {
00133 std::pair<base::State*, double> fail(xstate, 0.0);
00134 bool keep = si_->checkMotion(existing->state, xstate, fail);
00135 if (!keep && fail.second > minValidPathFraction_)
00136 keep = true;
00137
00138 if (keep)
00139 {
00140
00141 Motion *motion = new Motion(si_);
00142 si_->copyState(motion->state, xstate);
00143 motion->root = existing->root;
00144 motion->parent = existing;
00145
00146 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00147 disc.addMotion(motion, xcoord);
00148
00149 Discretization<Motion>::Cell* cellC = otherDisc.getGrid().getCell(xcoord);
00150
00151 if (cellC && !cellC->data->motions.empty())
00152 {
00153 Motion* connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
00154
00155 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root) &&
00156 si_->checkMotion(motion->state, connectOther->state))
00157 {
00158
00159
00160 std::vector<Motion*> mpath1;
00161 while (motion != NULL)
00162 {
00163 mpath1.push_back(motion);
00164 motion = motion->parent;
00165 }
00166
00167 std::vector<Motion*> mpath2;
00168 while (connectOther != NULL)
00169 {
00170 mpath2.push_back(connectOther);
00171 connectOther = connectOther->parent;
00172 }
00173
00174 if (startTree)
00175 mpath1.swap(mpath2);
00176
00177 PathGeometric *path = new PathGeometric(si_);
00178 path->states.reserve(mpath1.size() + mpath2.size());
00179 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00180 path->states.push_back(si_->cloneState(mpath1[i]->state));
00181 for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00182 path->states.push_back(si_->cloneState(mpath2[i]->state));
00183
00184 goal->setDifference(0.0);
00185 goal->setSolutionPath(base::PathPtr(path));
00186 break;
00187 }
00188 }
00189
00190 ecell->data->score *= goodScoreFactor_;
00191 }
00192 else
00193 ecell->data->score *= badScoreFactor_;
00194 }
00195 disc.updateCell(ecell);
00196 }
00197
00198 si_->freeState(xstate);
00199
00200 msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
00201 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
00202 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
00203 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
00204
00205 return goal->isAchieved();
00206 }
00207
00208 void ompl::geometric::BKPIECE1::freeMotion(Motion *motion)
00209 {
00210 if (motion->state)
00211 si_->freeState(motion->state);
00212 delete motion;
00213 }
00214
00215 void ompl::geometric::BKPIECE1::clear(void)
00216 {
00217 Planner::clear();
00218
00219 sampler_.reset();
00220 dStart_.clear();
00221 dGoal_.clear();
00222 }
00223
00224 void ompl::geometric::BKPIECE1::getPlannerData(base::PlannerData &data) const
00225 {
00226 Planner::getPlannerData(data);
00227 dStart_.getPlannerData(data, 1);
00228 dGoal_.getPlannerData(data, 2);
00229 }