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