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