All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
LBKPIECE1.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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/geometric/planners/kpiece/LBKPIECE1.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <cassert>
00041 
00042 void ompl::geometric::LBKPIECE1::setup(void)
00043 {
00044     Planner::setup();
00045     SelfConfig sc(si_, getName());
00046     sc.configureProjectionEvaluator(projectionEvaluator_);
00047     sc.configurePlannerRange(maxDistance_);
00048 
00049     if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00050         throw Exception("The minimum valid path fraction must be in the range (0,1]");
00051 
00052     dStart_.setDimension(projectionEvaluator_->getDimension());
00053     dGoal_.setDimension(projectionEvaluator_->getDimension());
00054 }
00055 
00056 bool ompl::geometric::LBKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00057 {
00058     checkValidity();
00059     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00060 
00061     if (!goal)
00062     {
00063         msg_.error("Unknown type of goal (or goal undefined)");
00064         return false;
00065     }
00066 
00067     Discretization<Motion>::Coord xcoord;
00068 
00069     while (const base::State *st = pis_.nextStart())
00070     {
00071         Motion* motion = new Motion(si_);
00072         si_->copyState(motion->state, st);
00073         motion->root = st;
00074         motion->valid = true;
00075         projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00076         dStart_.addMotion(motion, xcoord);
00077     }
00078 
00079     if (dStart_.getMotionCount() == 0)
00080     {
00081         msg_.error("Motion planning start tree could not be initialized!");
00082         return false;
00083     }
00084 
00085     if (!goal->canSample())
00086     {
00087         msg_.error("Insufficient states in sampleable goal region");
00088         return false;
00089     }
00090 
00091     if (!sampler_)
00092         sampler_ = si_->allocStateSampler();
00093 
00094     msg_.inform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
00095 
00096     base::State *xstate = si_->allocState();
00097     bool      startTree = true;
00098 
00099     while (ptc() == false)
00100     {
00101         Discretization<Motion> &disc      = startTree ? dStart_ : dGoal_;
00102         startTree = !startTree;
00103         Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
00104         disc.countIteration();
00105 
00106         // if we have not sampled too many goals already
00107         if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
00108         {
00109             const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00110             if (st)
00111             {
00112                 Motion* motion = new Motion(si_);
00113                 si_->copyState(motion->state, st);
00114                 motion->root = motion->state;
00115                 motion->valid = true;
00116                 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00117                 dGoal_.addMotion(motion, xcoord);
00118             }
00119             if (dGoal_.getMotionCount() == 0)
00120             {
00121                 msg_.error("Unable to sample any valid states for goal tree");
00122                 break;
00123             }
00124         }
00125 
00126         Discretization<Motion>::Cell *ecell    = NULL;
00127         Motion                       *existing = NULL;
00128         disc.selectMotion(existing, ecell);
00129         assert(existing);
00130         sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
00131 
00132         /* create a motion */
00133         Motion* motion = new Motion(si_);
00134         si_->copyState(motion->state, xstate);
00135         motion->parent = existing;
00136         motion->root = existing->root;
00137         existing->children.push_back(motion);
00138         projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00139         disc.addMotion(motion, xcoord);
00140 
00141         /* attempt to connect trees */
00142         Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
00143         if (ocell && !ocell->data->motions.empty())
00144         {
00145             Motion* connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
00146 
00147             if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
00148             {
00149                 Motion* connect = new Motion(si_);
00150                 si_->copyState(connect->state, connectOther->state);
00151                 connect->parent = motion;
00152                 connect->root = motion->root;
00153                 motion->children.push_back(connect);
00154                 projectionEvaluator_->computeCoordinates(connect->state, xcoord);
00155                 disc.addMotion(connect, xcoord);
00156 
00157                 if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
00158                 {
00159                     /* extract the motions and put them in solution vector */
00160 
00161                     std::vector<Motion*> mpath1;
00162                     while (motion != NULL)
00163                     {
00164                         mpath1.push_back(motion);
00165                         motion = motion->parent;
00166                     }
00167 
00168                     std::vector<Motion*> mpath2;
00169                     while (connectOther != NULL)
00170                     {
00171                         mpath2.push_back(connectOther);
00172                         connectOther = connectOther->parent;
00173                     }
00174 
00175                     if (startTree)
00176                         mpath1.swap(mpath2);
00177 
00178                     PathGeometric *path = new PathGeometric(si_);
00179                     path->states.reserve(mpath1.size() + mpath2.size());
00180                     for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00181                         path->states.push_back(si_->cloneState(mpath1[i]->state));
00182                     for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00183                         path->states.push_back(si_->cloneState(mpath2[i]->state));
00184 
00185                     goal->setDifference(0.0);
00186                     goal->setSolutionPath(base::PathPtr(path));
00187                     break;
00188                 }
00189             }
00190         }
00191     }
00192 
00193     si_->freeState(xstate);
00194 
00195     msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
00196                 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
00197                 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
00198                 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
00199 
00200     return goal->isAchieved();
00201 }
00202 
00203 bool ompl::geometric::LBKPIECE1::isPathValid(Discretization<Motion> &disc, Motion *motion, base::State *temp)
00204 {
00205     std::vector<Motion*> mpath;
00206 
00207     /* construct the solution path */
00208     while (motion != NULL)
00209     {
00210         mpath.push_back(motion);
00211         motion = motion->parent;
00212     }
00213 
00214     std::pair<base::State*, double> lastValid;
00215     lastValid.first = temp;
00216 
00217     /* check the path */
00218     for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00219         if (!mpath[i]->valid)
00220         {
00221             if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
00222                 mpath[i]->valid = true;
00223             else
00224             {
00225                 Motion *parent = mpath[i]->parent;
00226                 removeMotion(disc, mpath[i]);
00227 
00228                 // add the valid part of the path, if sufficiently long
00229                 if (lastValid.second > minValidPathFraction_)
00230                 {
00231                     Motion* reAdd = new Motion(si_);
00232                     si_->copyState(reAdd->state, lastValid.first);
00233                     reAdd->parent = parent;
00234                     reAdd->root = parent->root;
00235                     parent->children.push_back(reAdd);
00236                     reAdd->valid = true;
00237                     Discretization<Motion>::Coord coord;
00238                     projectionEvaluator_->computeCoordinates(reAdd->state, coord);
00239                     disc.addMotion(reAdd, coord);
00240                 }
00241 
00242                 return false;
00243             }
00244         }
00245     return true;
00246 }
00247 
00248 void ompl::geometric::LBKPIECE1::removeMotion(Discretization<Motion> &disc, Motion *motion)
00249 {
00250     /* remove from grid */
00251 
00252     Discretization<Motion>::Coord coord;
00253     projectionEvaluator_->computeCoordinates(motion->state, coord);
00254     disc.removeMotion(motion, coord);
00255 
00256     /* remove self from parent list */
00257 
00258     if (motion->parent)
00259     {
00260         for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00261             if (motion->parent->children[i] == motion)
00262             {
00263                 motion->parent->children.erase(motion->parent->children.begin() + i);
00264                 break;
00265             }
00266     }
00267 
00268     /* remove children */
00269     for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00270     {
00271         motion->children[i]->parent = NULL;
00272         removeMotion(disc, motion->children[i]);
00273     }
00274 
00275     freeMotion(motion);
00276 }
00277 
00278 
00279 void ompl::geometric::LBKPIECE1::freeMotion(Motion *motion)
00280 {
00281     if (motion->state)
00282         si_->freeState(motion->state);
00283     delete motion;
00284 }
00285 
00286 void ompl::geometric::LBKPIECE1::clear(void)
00287 {
00288     Planner::clear();
00289 
00290     sampler_.reset();
00291     dStart_.clear();
00292     dGoal_.clear();
00293 }
00294 
00295 void ompl::geometric::LBKPIECE1::getPlannerData(base::PlannerData &data) const
00296 {
00297     Planner::getPlannerData(data);
00298     dStart_.getPlannerData(data, 1);
00299     dGoal_.getPlannerData(data, 2);
00300 }