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