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/KPIECE1.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include <limits>
00040 #include <cassert>
00041
00042 void ompl::geometric::KPIECE1::setup(void)
00043 {
00044 Planner::setup();
00045 checkProjectionEvaluator(this, projectionEvaluator_);
00046 checkMotionLength(this, maxDistance_);
00047
00048 if (badScoreFactor_ < std::numeric_limits<double>::epsilon() || badScoreFactor_ > 1.0)
00049 throw Exception("Bad cell score factor must be in the range (0,1]");
00050 if (goodScoreFactor_ < std::numeric_limits<double>::epsilon() || goodScoreFactor_ > 1.0)
00051 throw Exception("Good cell score factor must be in the range (0,1]");
00052 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00053 throw Exception("The minimum valid path fraction must be in the range (0,1]");
00054
00055 disc_.setDimension(projectionEvaluator_->getDimension());
00056 }
00057
00058 void ompl::geometric::KPIECE1::clear(void)
00059 {
00060 Planner::clear();
00061 sampler_.reset();
00062 disc_.clear();
00063 }
00064
00065 void ompl::geometric::KPIECE1::freeMotion(Motion *motion)
00066 {
00067 if (motion->state)
00068 si_->freeState(motion->state);
00069 delete motion;
00070 }
00071
00072 bool ompl::geometric::KPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00073 {
00074 checkValidity();
00075 base::Goal *goal = pdef_->getGoal().get();
00076 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00077
00078 if (!goal)
00079 {
00080 msg_.error("Goal undefined");
00081 return false;
00082 }
00083
00084 Discretization<Motion>::Coord xcoord;
00085
00086 while (const base::State *st = pis_.nextStart())
00087 {
00088 Motion *motion = new Motion(si_);
00089 si_->copyState(motion->state, st);
00090 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00091 disc_.addMotion(motion, xcoord, 1.0);
00092 }
00093
00094 if (disc_.getMotionCount() == 0)
00095 {
00096 msg_.error("There are no valid initial states!");
00097 return false;
00098 }
00099
00100 if (!sampler_)
00101 sampler_ = si_->allocManifoldStateSampler();
00102
00103 msg_.inform("Starting with %u states", disc_.getMotionCount());
00104
00105 Motion *solution = NULL;
00106 Motion *approxsol = NULL;
00107 double approxdif = std::numeric_limits<double>::infinity();
00108 base::State *xstate = si_->allocState();
00109
00110 while (ptc() == false)
00111 {
00112 disc_.countIteration();
00113
00114
00115 Motion *existing = NULL;
00116 Discretization<Motion>::Cell *ecell = NULL;
00117 disc_.selectMotion(existing, ecell);
00118 assert(existing);
00119
00120
00121 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00122 goal_s->sampleGoal(xstate);
00123 else
00124 sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
00125
00126 std::pair<base::State*, double> fail(xstate, 0.0);
00127 bool keep = si_->checkMotion(existing->state, xstate, fail);
00128 if (!keep && fail.second > minValidPathFraction_)
00129 keep = true;
00130
00131 if (keep)
00132 {
00133
00134 Motion *motion = new Motion(si_);
00135 si_->copyState(motion->state, xstate);
00136 motion->parent = existing;
00137
00138 double dist = 0.0;
00139 bool solved = goal->isSatisfied(motion->state, &dist);
00140 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00141 disc_.addMotion(motion, xcoord, dist);
00142
00143 if (solved)
00144 {
00145 approxdif = dist;
00146 solution = motion;
00147 break;
00148 }
00149 if (dist < approxdif)
00150 {
00151 approxdif = dist;
00152 approxsol = motion;
00153 }
00154 ecell->data->score *= goodScoreFactor_;
00155 }
00156 else
00157 ecell->data->score *= badScoreFactor_;
00158
00159 disc_.updateCell(ecell);
00160 }
00161
00162 bool approximate = false;
00163 if (solution == NULL)
00164 {
00165 solution = approxsol;
00166 approximate = true;
00167 }
00168
00169 if (solution != NULL)
00170 {
00171
00172 std::vector<Motion*> mpath;
00173 while (solution != NULL)
00174 {
00175 mpath.push_back(solution);
00176 solution = solution->parent;
00177 }
00178
00179
00180 PathGeometric *path = new PathGeometric(si_);
00181 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00182 path->states.push_back(si_->cloneState(mpath[i]->state));
00183 goal->setDifference(approxdif);
00184 goal->setSolutionPath(base::PathPtr(path), approximate);
00185
00186 if (approximate)
00187 msg_.warn("Found approximate solution");
00188 }
00189
00190 si_->freeState(xstate);
00191
00192 msg_.inform("Created %u states in %u cells (%u internal + %u external)", disc_.getMotionCount(), disc_.getCellCount(),
00193 disc_.getGrid().countInternal(), disc_.getGrid().countExternal());
00194
00195 return goal->isAchieved();
00196 }
00197
00198 void ompl::geometric::KPIECE1::getPlannerData(base::PlannerData &data) const
00199 {
00200 Planner::getPlannerData(data);
00201 disc_.getPlannerData(data, 0);
00202 }