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