All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
KPIECE1.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "ompl/control/planners/kpiece/KPIECE1.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include "ompl/util/Exception.h"
00041 #include <limits>
00042 #include <cassert>
00043 
00044 ompl::control::KPIECE1::KPIECE1(const SpaceInformationPtr &si) : base::Planner(si, "KPIECE1")
00045 {
00046     specs_.approximateSolutions = true;
00047 
00048     siC_ = si.get();
00049     nCloseSamples_ = 30;
00050     goalBias_ = 0.05;
00051     selectBorderFraction_ = 0.8;
00052     badScoreFactor_ = 0.45;
00053     goodScoreFactor_ = 0.9;
00054     tree_.grid.onCellUpdate(computeImportance, NULL);
00055 
00056     Planner::declareParam<double>("goal_bias", this, &KPIECE1::setGoalBias, &KPIECE1::getGoalBias);
00057     Planner::declareParam<double>("border_fraction", this, &KPIECE1::setBorderFraction, &KPIECE1::getBorderFraction);
00058     Planner::declareParam<unsigned int>("max_close_samples", this, &KPIECE1::setMaxCloseSamplesCount, &KPIECE1::getMaxCloseSamplesCount);
00059     Planner::declareParam<double>("bad_score_factor", this, &KPIECE1::setBadCellScoreFactor, &KPIECE1::getBadCellScoreFactor);
00060     Planner::declareParam<double>("good_score_factor", this, &KPIECE1::setGoodCellScoreFactor, &KPIECE1::getGoodCellScoreFactor);
00061 }
00062 
00063 ompl::control::KPIECE1::~KPIECE1(void)
00064 {
00065     freeMemory();
00066 }
00067 
00068 void ompl::control::KPIECE1::setup(void)
00069 {
00070     Planner::setup();
00071     tools::SelfConfig sc(si_, getName());
00072     sc.configureProjectionEvaluator(projectionEvaluator_);
00073 
00074     if (badScoreFactor_ < std::numeric_limits<double>::epsilon() || badScoreFactor_ > 1.0)
00075         throw Exception("Bad cell score factor must be in the range (0,1]");
00076     if (goodScoreFactor_ < std::numeric_limits<double>::epsilon() || goodScoreFactor_ > 1.0)
00077         throw Exception("Good cell score factor must be in the range (0,1]");
00078     if (selectBorderFraction_ < std::numeric_limits<double>::epsilon() || selectBorderFraction_ > 1.0)
00079         throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
00080 
00081     tree_.grid.setDimension(projectionEvaluator_->getDimension());
00082 }
00083 
00084 void ompl::control::KPIECE1::clear(void)
00085 {
00086     Planner::clear();
00087     controlSampler_.reset();
00088     freeMemory();
00089     tree_.grid.clear();
00090     tree_.size = 0;
00091     tree_.iteration = 1;
00092 }
00093 
00094 void ompl::control::KPIECE1::freeMemory(void)
00095 {
00096     freeGridMotions(tree_.grid);
00097 }
00098 
00099 void ompl::control::KPIECE1::freeGridMotions(Grid &grid)
00100 {
00101     for (Grid::iterator it = grid.begin(); it != grid.end() ; ++it)
00102         freeCellData(it->second->data);
00103 }
00104 
00105 void ompl::control::KPIECE1::freeCellData(CellData *cdata)
00106 {
00107     for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
00108         freeMotion(cdata->motions[i]);
00109     delete cdata;
00110 }
00111 
00112 void ompl::control::KPIECE1::freeMotion(Motion *motion)
00113 {
00114     if (motion->state)
00115         si_->freeState(motion->state);
00116     if (motion->control)
00117         siC_->freeControl(motion->control);
00118     delete motion;
00119 }
00120 
00121 bool ompl::control::KPIECE1::CloseSamples::consider(Grid::Cell *cell, Motion *motion, double distance)
00122 {
00123     if (samples.empty())
00124     {
00125         CloseSample cs(cell, motion, distance);
00126         samples.insert(cs);
00127         return true;
00128     }
00129     // if the sample we're considering is closer to the goal than the worst sample in the
00130     // set of close samples, we include it
00131     if (samples.rbegin()->distance > distance)
00132     {
00133         // if the inclusion would go above the maximum allowed size,
00134         // remove the last element
00135         if (samples.size() >= maxSize)
00136             samples.erase(--samples.end());
00137         CloseSample cs(cell, motion, distance);
00138         samples.insert(cs);
00139         return true;
00140     }
00141 
00142     return false;
00143 }
00144 
00145 
00147 // this is the factor by which distances are inflated when considered for addition to closest samples
00148 static const double CLOSE_MOTION_DISTANCE_INFLATION_FACTOR = 1.1;
00150 
00151 bool ompl::control::KPIECE1::CloseSamples::selectMotion(Motion* &smotion, Grid::Cell* &scell)
00152 {
00153     if (samples.size() > 0)
00154     {
00155         scell = samples.begin()->cell;
00156         smotion = samples.begin()->motion;
00157         // average the highest & lowest distances and multiply by CLOSE_MOTION_DISTANCE_INFLATION_FACTOR
00158         // (make the distance appear artificially longer)
00159         double d = (samples.begin()->distance + samples.rbegin()->distance) * (CLOSE_MOTION_DISTANCE_INFLATION_FACTOR / 2.0);
00160         samples.erase(samples.begin());
00161         consider(scell, smotion, d);
00162         return true;
00163     }
00164     return false;
00165 }
00166 
00167 unsigned int ompl::control::KPIECE1::findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count)
00168 {
00169     for (unsigned int i = index + 1 ; i < count ; ++i)
00170         if (coords[i] != coords[index])
00171             return i - 1;
00172 
00173     return count - 1;
00174 }
00175 
00176 bool ompl::control::KPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00177 {
00178     checkValidity();
00179     base::Goal *goal = pdef_->getGoal().get();
00180 
00181     while (const base::State *st = pis_.nextStart())
00182     {
00183         Motion *motion = new Motion(siC_);
00184         si_->copyState(motion->state, st);
00185         siC_->nullControl(motion->control);
00186         addMotion(motion, 1.0);
00187     }
00188 
00189     if (tree_.grid.size() == 0)
00190     {
00191         msg_.error("There are no valid initial states!");
00192         return false;
00193     }
00194 
00195     if (!controlSampler_)
00196         controlSampler_ = siC_->allocControlSampler();
00197 
00198     msg_.inform("Starting with %u states", tree_.size);
00199 
00200     Motion *solution  = NULL;
00201     Motion *approxsol = NULL;
00202     double  approxdif = std::numeric_limits<double>::infinity();
00203 
00204     Control *rctrl = siC_->allocControl();
00205 
00206     std::vector<base::State*> states(siC_->getMaxControlDuration() + 1);
00207     std::vector<Grid::Coord>  coords(states.size());
00208     std::vector<Grid::Cell*>  cells(coords.size());
00209 
00210     for (unsigned int i = 0 ; i < states.size() ; ++i)
00211         states[i] = si_->allocState();
00212 
00213     // samples that were found to be the best, so far
00214     CloseSamples closeSamples(nCloseSamples_);
00215 
00216     while (ptc() == false)
00217     {
00218         tree_.iteration++;
00219 
00220         /* Decide on a state to expand from */
00221         Motion     *existing = NULL;
00222         Grid::Cell *ecell = NULL;
00223 
00224         if (closeSamples.canSample() && rng_.uniform01() < goalBias_)
00225         {
00226             if (!closeSamples.selectMotion(existing, ecell))
00227                 selectMotion(existing, ecell);
00228         }
00229         else
00230             selectMotion(existing, ecell);
00231         assert(existing);
00232 
00233         /* sample a random control */
00234         controlSampler_->sampleNext(rctrl, existing->control, existing->state);
00235 
00236         /* propagate */
00237         unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
00238         cd = siC_->propagateWhileValid(existing->state, rctrl, cd, states, false);
00239 
00240         /* if we have enough steps */
00241         if (cd >= siC_->getMinControlDuration())
00242         {
00243             std::size_t avgCov_two_thirds = (2 * tree_.size) / (3 * tree_.grid.size());
00244             bool interestingMotion = false;
00245 
00246             // split the motion into smaller ones, so we do not cross cell boundaries
00247             for (unsigned int i = 0 ; i < cd ; ++i)
00248             {
00249                 projectionEvaluator_->computeCoordinates(states[i], coords[i]);
00250                 cells[i] = tree_.grid.getCell(coords[i]);
00251                 if (!cells[i])
00252                     interestingMotion = true;
00253                 else
00254                 {
00255                     if (!interestingMotion && cells[i]->data->motions.size() <= avgCov_two_thirds)
00256                         interestingMotion = true;
00257                 }
00258             }
00259 
00260             if (interestingMotion || rng_.uniform01() < 0.05)
00261             {
00262                 unsigned int index = 0;
00263                 while (index < cd)
00264                 {
00265                     unsigned int nextIndex = findNextMotion(coords, index, cd);
00266                     Motion *motion = new Motion(siC_);
00267                     si_->copyState(motion->state, states[nextIndex]);
00268                     siC_->copyControl(motion->control, rctrl);
00269                     motion->steps = nextIndex - index + 1;
00270                     motion->parent = existing;
00271 
00272                     double dist = 0.0;
00273                     bool solv = goal->isSatisfied(motion->state, &dist);
00274                     Grid::Cell *toCell = addMotion(motion, dist);
00275 
00276                     if (solv)
00277                     {
00278                         approxdif = dist;
00279                         solution = motion;
00280                         break;
00281                     }
00282                     if (dist < approxdif)
00283                     {
00284                         approxdif = dist;
00285                         approxsol = motion;
00286                     }
00287 
00288                     closeSamples.consider(toCell, motion, dist);
00289 
00290                     // new parent will be the newly created motion
00291                     existing = motion;
00292                     index = nextIndex + 1;
00293                 }
00294 
00295                 if (solution)
00296                     break;
00297             }
00298 
00299             // update cell score
00300             ecell->data->score *= goodScoreFactor_;
00301         }
00302         else
00303             ecell->data->score *= badScoreFactor_;
00304 
00305         tree_.grid.update(ecell);
00306     }
00307 
00308     bool solved = false;
00309     bool approximate = false;
00310     if (solution == NULL)
00311     {
00312         solution = approxsol;
00313         approximate = true;
00314     }
00315 
00316     if (solution != NULL)
00317     {
00318         /* construct the solution path */
00319         std::vector<Motion*> mpath;
00320         while (solution != NULL)
00321         {
00322             mpath.push_back(solution);
00323             solution = solution->parent;
00324         }
00325 
00326         /* set the solution path */
00327         PathControl *path = new PathControl(si_);
00328         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00329             if (mpath[i]->parent)
00330                 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
00331             else
00332                 path->append(mpath[i]->state);
00333 
00334         goal->addSolutionPath(base::PathPtr(path), approximate, approxdif);
00335         solved = true;
00336     }
00337 
00338     siC_->freeControl(rctrl);
00339     for (unsigned int i = 0 ; i < states.size() ; ++i)
00340         si_->freeState(states[i]);
00341 
00342     msg_.inform("Created %u states in %u cells (%u internal + %u external)", tree_.size, tree_.grid.size(),
00343                  tree_.grid.countInternal(), tree_.grid.countExternal());
00344 
00345     return solved;
00346 }
00347 
00348 bool ompl::control::KPIECE1::selectMotion(Motion* &smotion, Grid::Cell* &scell)
00349 {
00350     scell = rng_.uniform01() < std::max(selectBorderFraction_, tree_.grid.fracExternal()) ?
00351         tree_.grid.topExternal() : tree_.grid.topInternal();
00352 
00353     // We are running on finite precision, so our update scheme will end up
00354     // with 0 values for the score. This is where we fix the problem
00355     if (scell->data->score < std::numeric_limits<double>::epsilon())
00356     {
00357         msg_.debug("Numerical precision limit reached. Resetting costs.");
00358         std::vector<CellData*> content;
00359         content.reserve(tree_.grid.size());
00360         tree_.grid.getContent(content);
00361         for (std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
00362             (*it)->score += 1.0 + log((double)((*it)->iteration));
00363         tree_.grid.updateAll();
00364     }
00365 
00366     if (scell && !scell->data->motions.empty())
00367     {
00368         scell->data->selections++;
00369         smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
00370         return true;
00371     }
00372     else
00373         return false;
00374 }
00375 
00377 // this is the offset added to estimated distances to the goal, so we avoid division by 0
00378 static const double DISTANCE_TO_GOAL_OFFSET = 1e-3;
00380 
00381 ompl::control::KPIECE1::Grid::Cell* ompl::control::KPIECE1::addMotion(Motion *motion, double dist)
00382 {
00383     Grid::Coord coord;
00384     projectionEvaluator_->computeCoordinates(motion->state, coord);
00385     Grid::Cell* cell = tree_.grid.getCell(coord);
00386     if (cell)
00387     {
00388         cell->data->motions.push_back(motion);
00389         cell->data->coverage += motion->steps;
00390         tree_.grid.update(cell);
00391     }
00392     else
00393     {
00394         cell = tree_.grid.createCell(coord);
00395         cell->data = new CellData();
00396         cell->data->motions.push_back(motion);
00397         cell->data->coverage = motion->steps;
00398         cell->data->iteration = tree_.iteration;
00399         cell->data->selections = 1;
00400         cell->data->score = (1.0 + log((double)(tree_.iteration))) / (DISTANCE_TO_GOAL_OFFSET + dist);
00401         tree_.grid.add(cell);
00402     }
00403     tree_.size++;
00404     return cell;
00405 }
00406 
00407 void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const
00408 {
00409     Planner::getPlannerData(data);
00410 
00411     Grid::CellArray cells;
00412     tree_.grid.getCells(cells);
00413 
00414     if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
00415     {
00416         double delta = siC_->getPropagationStepSize();
00417 
00418         for (unsigned int i = 0 ; i < cells.size() ; ++i)
00419             for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
00420             {
00421                 const Motion* m = cells[i]->data->motions[j];
00422                 if (m->parent)
00423                     cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
00424                 else
00425                     cpd->recordEdge(NULL, m->state, NULL, 0.);
00426                 cpd->tagState(m->state, cells[i]->border ? 2 : 1);
00427             }
00428     }
00429     else
00430     {
00431         for (unsigned int i = 0 ; i < cells.size() ; ++i)
00432             for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
00433             {
00434                 const Motion* m = cells[i]->data->motions[j];
00435                 data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
00436                 data.tagState(m->state, cells[i]->border ? 2 : 1);
00437             }
00438     }
00439 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends