ompl/control/planners/kpiece/src/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/goals/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     lastGoalMotion_ = NULL;
00056 
00057     Planner::declareParam<double>("goal_bias", this, &KPIECE1::setGoalBias, &KPIECE1::getGoalBias, "0.:.05:1.");
00058     Planner::declareParam<double>("border_fraction", this, &KPIECE1::setBorderFraction, &KPIECE1::getBorderFraction, "0.:0.05:1.");
00059     Planner::declareParam<unsigned int>("max_close_samples", this, &KPIECE1::setMaxCloseSamplesCount, &KPIECE1::getMaxCloseSamplesCount);
00060     Planner::declareParam<double>("bad_score_factor", this, &KPIECE1::setBadCellScoreFactor, &KPIECE1::getBadCellScoreFactor);
00061     Planner::declareParam<double>("good_score_factor", this, &KPIECE1::setGoodCellScoreFactor, &KPIECE1::getGoodCellScoreFactor);
00062 }
00063 
00064 ompl::control::KPIECE1::~KPIECE1()
00065 {
00066     freeMemory();
00067 }
00068 
00069 void ompl::control::KPIECE1::setup()
00070 {
00071     Planner::setup();
00072     tools::SelfConfig sc(si_, getName());
00073     sc.configureProjectionEvaluator(projectionEvaluator_);
00074 
00075     if (badScoreFactor_ < std::numeric_limits<double>::epsilon() || badScoreFactor_ > 1.0)
00076         throw Exception("Bad cell score factor must be in the range (0,1]");
00077     if (goodScoreFactor_ < std::numeric_limits<double>::epsilon() || goodScoreFactor_ > 1.0)
00078         throw Exception("Good cell score factor must be in the range (0,1]");
00079     if (selectBorderFraction_ < std::numeric_limits<double>::epsilon() || selectBorderFraction_ > 1.0)
00080         throw Exception("The fraction of time spent selecting border cells must be in the range (0,1]");
00081 
00082     tree_.grid.setDimension(projectionEvaluator_->getDimension());
00083 }
00084 
00085 void ompl::control::KPIECE1::clear()
00086 {
00087     Planner::clear();
00088     controlSampler_.reset();
00089     freeMemory();
00090     tree_.grid.clear();
00091     tree_.size = 0;
00092     tree_.iteration = 1;
00093     lastGoalMotion_ = NULL;
00094 }
00095 
00096 void ompl::control::KPIECE1::freeMemory()
00097 {
00098     freeGridMotions(tree_.grid);
00099 }
00100 
00101 void ompl::control::KPIECE1::freeGridMotions(Grid &grid)
00102 {
00103     for (Grid::iterator it = grid.begin(); it != grid.end() ; ++it)
00104         freeCellData(it->second->data);
00105 }
00106 
00107 void ompl::control::KPIECE1::freeCellData(CellData *cdata)
00108 {
00109     for (unsigned int i = 0 ; i < cdata->motions.size() ; ++i)
00110         freeMotion(cdata->motions[i]);
00111     delete cdata;
00112 }
00113 
00114 void ompl::control::KPIECE1::freeMotion(Motion *motion)
00115 {
00116     if (motion->state)
00117         si_->freeState(motion->state);
00118     if (motion->control)
00119         siC_->freeControl(motion->control);
00120     delete motion;
00121 }
00122 
00123 bool ompl::control::KPIECE1::CloseSamples::consider(Grid::Cell *cell, Motion *motion, double distance)
00124 {
00125     if (samples.empty())
00126     {
00127         CloseSample cs(cell, motion, distance);
00128         samples.insert(cs);
00129         return true;
00130     }
00131     // if the sample we're considering is closer to the goal than the worst sample in the
00132     // set of close samples, we include it
00133     if (samples.rbegin()->distance > distance)
00134     {
00135         // if the inclusion would go above the maximum allowed size,
00136         // remove the last element
00137         if (samples.size() >= maxSize)
00138             samples.erase(--samples.end());
00139         CloseSample cs(cell, motion, distance);
00140         samples.insert(cs);
00141         return true;
00142     }
00143 
00144     return false;
00145 }
00146 
00147 
00149 // this is the factor by which distances are inflated when considered for addition to closest samples
00150 static const double CLOSE_MOTION_DISTANCE_INFLATION_FACTOR = 1.1;
00152 
00153 bool ompl::control::KPIECE1::CloseSamples::selectMotion(Motion* &smotion, Grid::Cell* &scell)
00154 {
00155     if (samples.size() > 0)
00156     {
00157         scell = samples.begin()->cell;
00158         smotion = samples.begin()->motion;
00159         // average the highest & lowest distances and multiply by CLOSE_MOTION_DISTANCE_INFLATION_FACTOR
00160         // (make the distance appear artificially longer)
00161         double d = (samples.begin()->distance + samples.rbegin()->distance) * (CLOSE_MOTION_DISTANCE_INFLATION_FACTOR / 2.0);
00162         samples.erase(samples.begin());
00163         consider(scell, smotion, d);
00164         return true;
00165     }
00166     return false;
00167 }
00168 
00169 unsigned int ompl::control::KPIECE1::findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count)
00170 {
00171     for (unsigned int i = index + 1 ; i < count ; ++i)
00172         if (coords[i] != coords[index])
00173             return i - 1;
00174 
00175     return count - 1;
00176 }
00177 
00178 ompl::base::PlannerStatus ompl::control::KPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00179 {
00180     checkValidity();
00181     base::Goal *goal = pdef_->getGoal().get();
00182 
00183     while (const base::State *st = pis_.nextStart())
00184     {
00185         Motion *motion = new Motion(siC_);
00186         si_->copyState(motion->state, st);
00187         siC_->nullControl(motion->control);
00188         addMotion(motion, 1.0);
00189     }
00190 
00191     if (tree_.grid.size() == 0)
00192     {
00193         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00194         return base::PlannerStatus::INVALID_START;
00195     }
00196 
00197     if (!controlSampler_)
00198         controlSampler_ = siC_->allocControlSampler();
00199 
00200     OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size);
00201 
00202     Motion *solution  = NULL;
00203     Motion *approxsol = NULL;
00204     double  approxdif = std::numeric_limits<double>::infinity();
00205 
00206     Control *rctrl = siC_->allocControl();
00207 
00208     std::vector<base::State*> states(siC_->getMaxControlDuration() + 1);
00209     std::vector<Grid::Coord>  coords(states.size());
00210     std::vector<Grid::Cell*>  cells(coords.size());
00211 
00212     for (unsigned int i = 0 ; i < states.size() ; ++i)
00213         states[i] = si_->allocState();
00214 
00215     // samples that were found to be the best, so far
00216     CloseSamples closeSamples(nCloseSamples_);
00217 
00218     while (ptc == false)
00219     {
00220         tree_.iteration++;
00221 
00222         /* Decide on a state to expand from */
00223         Motion     *existing = NULL;
00224         Grid::Cell *ecell = NULL;
00225 
00226         if (closeSamples.canSample() && rng_.uniform01() < goalBias_)
00227         {
00228             if (!closeSamples.selectMotion(existing, ecell))
00229                 selectMotion(existing, ecell);
00230         }
00231         else
00232             selectMotion(existing, ecell);
00233         assert(existing);
00234 
00235         /* sample a random control */
00236         controlSampler_->sampleNext(rctrl, existing->control, existing->state);
00237 
00238         /* propagate */
00239         unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
00240         cd = siC_->propagateWhileValid(existing->state, rctrl, cd, states, false);
00241 
00242         /* if we have enough steps */
00243         if (cd >= siC_->getMinControlDuration())
00244         {
00245             std::size_t avgCov_two_thirds = (2 * tree_.size) / (3 * tree_.grid.size());
00246             bool interestingMotion = false;
00247 
00248             // split the motion into smaller ones, so we do not cross cell boundaries
00249             for (unsigned int i = 0 ; i < cd ; ++i)
00250             {
00251                 projectionEvaluator_->computeCoordinates(states[i], coords[i]);
00252                 cells[i] = tree_.grid.getCell(coords[i]);
00253                 if (!cells[i])
00254                     interestingMotion = true;
00255                 else
00256                 {
00257                     if (!interestingMotion && cells[i]->data->motions.size() <= avgCov_two_thirds)
00258                         interestingMotion = true;
00259                 }
00260             }
00261 
00262             if (interestingMotion || rng_.uniform01() < 0.05)
00263             {
00264                 unsigned int index = 0;
00265                 while (index < cd)
00266                 {
00267                     unsigned int nextIndex = findNextMotion(coords, index, cd);
00268                     Motion *motion = new Motion(siC_);
00269                     si_->copyState(motion->state, states[nextIndex]);
00270                     siC_->copyControl(motion->control, rctrl);
00271                     motion->steps = nextIndex - index + 1;
00272                     motion->parent = existing;
00273 
00274                     double dist = 0.0;
00275                     bool solv = goal->isSatisfied(motion->state, &dist);
00276                     Grid::Cell *toCell = addMotion(motion, dist);
00277 
00278                     if (solv)
00279                     {
00280                         approxdif = dist;
00281                         solution = motion;
00282                         break;
00283                     }
00284                     if (dist < approxdif)
00285                     {
00286                         approxdif = dist;
00287                         approxsol = motion;
00288                     }
00289 
00290                     closeSamples.consider(toCell, motion, dist);
00291 
00292                     // new parent will be the newly created motion
00293                     existing = motion;
00294                     index = nextIndex + 1;
00295                 }
00296 
00297                 if (solution)
00298                     break;
00299             }
00300 
00301             // update cell score
00302             ecell->data->score *= goodScoreFactor_;
00303         }
00304         else
00305             ecell->data->score *= badScoreFactor_;
00306 
00307         tree_.grid.update(ecell);
00308     }
00309 
00310     bool solved = false;
00311     bool approximate = false;
00312     if (solution == NULL)
00313     {
00314         solution = approxsol;
00315         approximate = true;
00316     }
00317 
00318     if (solution != NULL)
00319     {
00320         lastGoalMotion_ = solution;
00321 
00322         /* construct the solution path */
00323         std::vector<Motion*> mpath;
00324         while (solution != NULL)
00325         {
00326             mpath.push_back(solution);
00327             solution = solution->parent;
00328         }
00329 
00330         /* set the solution path */
00331         PathControl *path = new PathControl(si_);
00332         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00333             if (mpath[i]->parent)
00334                 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
00335             else
00336                 path->append(mpath[i]->state);
00337 
00338         pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
00339         solved = true;
00340     }
00341 
00342     siC_->freeControl(rctrl);
00343     for (unsigned int i = 0 ; i < states.size() ; ++i)
00344         si_->freeState(states[i]);
00345 
00346     OMPL_INFORM("%s: Created %u states in %u cells (%u internal + %u external)",
00347                 getName().c_str(), tree_.size, tree_.grid.size(),
00348                 tree_.grid.countInternal(), tree_.grid.countExternal());
00349 
00350     return base::PlannerStatus(solved, approximate);
00351 }
00352 
00353 bool ompl::control::KPIECE1::selectMotion(Motion* &smotion, Grid::Cell* &scell)
00354 {
00355     scell = rng_.uniform01() < std::max(selectBorderFraction_, tree_.grid.fracExternal()) ?
00356         tree_.grid.topExternal() : tree_.grid.topInternal();
00357 
00358     // We are running on finite precision, so our update scheme will end up
00359     // with 0 values for the score. This is where we fix the problem
00360     if (scell->data->score < std::numeric_limits<double>::epsilon())
00361     {
00362         OMPL_DEBUG("%s: Numerical precision limit reached. Resetting costs.", getName().c_str());
00363         std::vector<CellData*> content;
00364         content.reserve(tree_.grid.size());
00365         tree_.grid.getContent(content);
00366         for (std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
00367             (*it)->score += 1.0 + log((double)((*it)->iteration));
00368         tree_.grid.updateAll();
00369     }
00370 
00371     if (scell && !scell->data->motions.empty())
00372     {
00373         scell->data->selections++;
00374         smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
00375         return true;
00376     }
00377     else
00378         return false;
00379 }
00380 
00382 // this is the offset added to estimated distances to the goal, so we avoid division by 0
00383 static const double DISTANCE_TO_GOAL_OFFSET = 1e-3;
00385 
00386 ompl::control::KPIECE1::Grid::Cell* ompl::control::KPIECE1::addMotion(Motion *motion, double dist)
00387 {
00388     Grid::Coord coord;
00389     projectionEvaluator_->computeCoordinates(motion->state, coord);
00390     Grid::Cell* cell = tree_.grid.getCell(coord);
00391     if (cell)
00392     {
00393         cell->data->motions.push_back(motion);
00394         cell->data->coverage += motion->steps;
00395         tree_.grid.update(cell);
00396     }
00397     else
00398     {
00399         cell = tree_.grid.createCell(coord);
00400         cell->data = new CellData();
00401         cell->data->motions.push_back(motion);
00402         cell->data->coverage = motion->steps;
00403         cell->data->iteration = tree_.iteration;
00404         cell->data->selections = 1;
00405         cell->data->score = (1.0 + log((double)(tree_.iteration))) / (DISTANCE_TO_GOAL_OFFSET + dist);
00406         tree_.grid.add(cell);
00407     }
00408     tree_.size++;
00409     return cell;
00410 }
00411 
00412 void ompl::control::KPIECE1::getPlannerData(base::PlannerData &data) const
00413 {
00414     Planner::getPlannerData(data);
00415 
00416     Grid::CellArray cells;
00417     tree_.grid.getCells(cells);
00418 
00419     double delta = siC_->getPropagationStepSize();
00420 
00421     if (lastGoalMotion_)
00422         data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
00423 
00424     for (unsigned int i = 0 ; i < cells.size() ; ++i)
00425     {
00426         for (unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
00427         {
00428             const Motion *m = cells[i]->data->motions[j];
00429             if (m->parent)
00430             {
00431                 if (data.hasControls())
00432                     data.addEdge(base::PlannerDataVertex (m->parent->state),
00433                                  base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1),
00434                                  control::PlannerDataEdgeControl (m->control, m->steps * delta));
00435                 else
00436                     data.addEdge(base::PlannerDataVertex (m->parent->state),
00437                                  base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1));
00438             }
00439             else
00440                 data.addStartVertex(base::PlannerDataVertex (m->state, cells[i]->border ? 2 : 1));
00441 
00442             // A state created as a parent first may have an improper tag variable
00443             data.tagState(m->state, cells[i]->border ? 2 : 1);
00444         }
00445     }
00446 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines