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/est/EST.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <limits>
00041 #include <cassert>
00042
00043 ompl::geometric::EST::EST(const base::SpaceInformationPtr &si) : base::Planner(si, "EST")
00044 {
00045 specs_.approximateSolutions = true;
00046 specs_.directed = true;
00047 goalBias_ = 0.05;
00048 maxDistance_ = 0.0;
00049 lastGoalMotion_ = NULL;
00050
00051 Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange);
00052 Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias);
00053 }
00054
00055 ompl::geometric::EST::~EST(void)
00056 {
00057 freeMemory();
00058 }
00059
00060 void ompl::geometric::EST::setup(void)
00061 {
00062 Planner::setup();
00063 tools::SelfConfig sc(si_, getName());
00064 sc.configureProjectionEvaluator(projectionEvaluator_);
00065 sc.configurePlannerRange(maxDistance_);
00066
00067 tree_.grid.setDimension(projectionEvaluator_->getDimension());
00068 }
00069
00070 void ompl::geometric::EST::clear(void)
00071 {
00072 Planner::clear();
00073 sampler_.reset();
00074 freeMemory();
00075 tree_.grid.clear();
00076 tree_.size = 0;
00077 pdf_.clear();
00078 lastGoalMotion_ = NULL;
00079 }
00080
00081 void ompl::geometric::EST::freeMemory(void)
00082 {
00083 for (Grid<MotionInfo>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
00084 {
00085 for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
00086 {
00087 if (it->second->data[i]->state)
00088 si_->freeState(it->second->data[i]->state);
00089 delete it->second->data[i];
00090 }
00091 }
00092 }
00093
00094 ompl::base::PlannerStatus ompl::geometric::EST::solve(const base::PlannerTerminationCondition &ptc)
00095 {
00096 checkValidity();
00097 base::Goal *goal = pdef_->getGoal().get();
00098 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00099
00100 while (const base::State *st = pis_.nextStart())
00101 {
00102 Motion *motion = new Motion(si_);
00103 si_->copyState(motion->state, st);
00104 addMotion(motion);
00105 }
00106
00107 if (tree_.grid.size() == 0)
00108 {
00109 logError("There are no valid initial states!");
00110 return base::PlannerStatus::INVALID_START;
00111 }
00112
00113 if (!sampler_)
00114 sampler_ = si_->allocValidStateSampler();
00115
00116 logInform("Starting with %u states", tree_.size);
00117
00118 Motion *solution = NULL;
00119 Motion *approxsol = NULL;
00120 double approxdif = std::numeric_limits<double>::infinity();
00121 base::State *xstate = si_->allocState();
00122
00123 while (ptc() == false)
00124 {
00125
00126 Motion *existing = selectMotion();
00127 assert(existing);
00128
00129
00130 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00131 goal_s->sampleGoal(xstate);
00132 else
00133 if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
00134 continue;
00135
00136 if (si_->checkMotion(existing->state, xstate))
00137 {
00138
00139 Motion *motion = new Motion(si_);
00140 si_->copyState(motion->state, xstate);
00141 motion->parent = existing;
00142
00143 addMotion(motion);
00144 double dist = 0.0;
00145 bool solved = goal->isSatisfied(motion->state, &dist);
00146 if (solved)
00147 {
00148 approxdif = dist;
00149 solution = motion;
00150 break;
00151 }
00152 if (dist < approxdif)
00153 {
00154 approxdif = dist;
00155 approxsol = motion;
00156 }
00157 }
00158 }
00159
00160 bool solved = false;
00161 bool approximate = false;
00162 if (solution == NULL)
00163 {
00164 solution = approxsol;
00165 approximate = true;
00166 }
00167
00168 if (solution != NULL)
00169 {
00170 lastGoalMotion_ = solution;
00171
00172
00173 std::vector<Motion*> mpath;
00174 while (solution != NULL)
00175 {
00176 mpath.push_back(solution);
00177 solution = solution->parent;
00178 }
00179
00180
00181 PathGeometric *path = new PathGeometric(si_);
00182 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00183 path->append(mpath[i]->state);
00184 pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
00185 solved = true;
00186 }
00187
00188 si_->freeState(xstate);
00189
00190 logInform("Created %u states in %u cells", tree_.size, tree_.grid.size());
00191
00192 return base::PlannerStatus(solved, approximate);
00193 }
00194
00195 ompl::geometric::EST::Motion* ompl::geometric::EST::selectMotion(void)
00196 {
00197 GridCell* cell = pdf_.sample(rng_.uniform01());
00198 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
00199 }
00200
00201 void ompl::geometric::EST::addMotion(Motion *motion)
00202 {
00203 Grid<MotionInfo>::Coord coord;
00204 projectionEvaluator_->computeCoordinates(motion->state, coord);
00205 GridCell* cell = tree_.grid.getCell(coord);
00206 if (cell)
00207 {
00208 cell->data.push_back(motion);
00209 pdf_.update(cell->data.elem_, 1.0/cell->data.size());
00210 }
00211 else
00212 {
00213 cell = tree_.grid.createCell(coord);
00214 cell->data.push_back(motion);
00215 tree_.grid.add(cell);
00216 cell->data.elem_ = pdf_.add(cell, 1.0);
00217 }
00218 tree_.size++;
00219 }
00220
00221 void ompl::geometric::EST::getPlannerData(base::PlannerData &data) const
00222 {
00223 Planner::getPlannerData(data);
00224
00225 std::vector<MotionInfo> motions;
00226 tree_.grid.getContent(motions);
00227
00228 if (lastGoalMotion_)
00229 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
00230
00231 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00232 for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00233 {
00234 if (motions[i][j]->parent == NULL)
00235 data.addStartVertex(base::PlannerDataVertex(motions[i][j]->state));
00236 else
00237 data.addEdge(base::PlannerDataVertex(motions[i][j]->parent->state),
00238 base::PlannerDataVertex(motions[i][j]->state));
00239 }
00240 }