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