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