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, "0.:1.:10000");
00053     Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction, "0.:.05:1.");
00054     Planner::declareParam<double>("min_valid_path_fraction", this, &LBKPIECE1::setMinValidPathFraction, &LBKPIECE1::getMinValidPathFraction);
00055 }
00056 
00057  ompl::geometric::LBKPIECE1::~LBKPIECE1()
00058 {
00059 }
00060 
00061 void ompl::geometric::LBKPIECE1::setup()
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         OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
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         OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str());
00101         return base::PlannerStatus::INVALID_START;
00102     }
00103 
00104     if (!goal->couldSample())
00105     {
00106         OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
00107         return base::PlannerStatus::INVALID_GOAL;
00108     }
00109 
00110     if (!sampler_)
00111         sampler_ = si_->allocStateSampler();
00112 
00113     OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (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                 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str());
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(connectOther->state, motion->state);
00181                     else
00182                         connectionPoint_ = std::make_pair(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, getName());
00211                     solved = true;
00212                     break;
00213                 }
00214             }
00215         }
00216     }
00217 
00218     si_->freeState(xstate);
00219 
00220     OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
00221                 getName().c_str(),
00222                 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
00223                 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
00224                 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
00225 
00226     return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00227 }
00228 
00229 bool ompl::geometric::LBKPIECE1::isPathValid(Discretization<Motion> &disc, Motion *motion, base::State *temp)
00230 {
00231     std::vector<Motion*> mpath;
00232 
00233     /* construct the solution path */
00234     while (motion != NULL)
00235     {
00236         mpath.push_back(motion);
00237         motion = motion->parent;
00238     }
00239 
00240     std::pair<base::State*, double> lastValid;
00241     lastValid.first = temp;
00242 
00243     /* check the path */
00244     for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00245         if (!mpath[i]->valid)
00246         {
00247             if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
00248                 mpath[i]->valid = true;
00249             else
00250             {
00251                 Motion *parent = mpath[i]->parent;
00252                 removeMotion(disc, mpath[i]);
00253 
00254                 // add the valid part of the path, if sufficiently long
00255                 if (lastValid.second > minValidPathFraction_)
00256                 {
00257                     Motion *reAdd = new Motion(si_);
00258                     si_->copyState(reAdd->state, lastValid.first);
00259                     reAdd->parent = parent;
00260                     reAdd->root = parent->root;
00261                     parent->children.push_back(reAdd);
00262                     reAdd->valid = true;
00263                     Discretization<Motion>::Coord coord;
00264                     projectionEvaluator_->computeCoordinates(reAdd->state, coord);
00265                     disc.addMotion(reAdd, coord);
00266                 }
00267 
00268                 return false;
00269             }
00270         }
00271     return true;
00272 }
00273 
00274 void ompl::geometric::LBKPIECE1::removeMotion(Discretization<Motion> &disc, Motion *motion)
00275 {
00276     /* remove from grid */
00277 
00278     Discretization<Motion>::Coord coord;
00279     projectionEvaluator_->computeCoordinates(motion->state, coord);
00280     disc.removeMotion(motion, coord);
00281 
00282     /* remove self from parent list */
00283 
00284     if (motion->parent)
00285     {
00286         for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00287             if (motion->parent->children[i] == motion)
00288             {
00289                 motion->parent->children.erase(motion->parent->children.begin() + i);
00290                 break;
00291             }
00292     }
00293 
00294     /* remove children */
00295     for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00296     {
00297         motion->children[i]->parent = NULL;
00298         removeMotion(disc, motion->children[i]);
00299     }
00300 
00301     freeMotion(motion);
00302 }
00303 
00304 
00305 void ompl::geometric::LBKPIECE1::freeMotion(Motion *motion)
00306 {
00307     if (motion->state)
00308         si_->freeState(motion->state);
00309     delete motion;
00310 }
00311 
00312 void ompl::geometric::LBKPIECE1::clear()
00313 {
00314     Planner::clear();
00315 
00316     sampler_.reset();
00317     dStart_.clear();
00318     dGoal_.clear();
00319     connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
00320 }
00321 
00322 void ompl::geometric::LBKPIECE1::getPlannerData(base::PlannerData &data) const
00323 {
00324     Planner::getPlannerData(data);
00325     dStart_.getPlannerData(data, 1, true, NULL);
00326     dGoal_.getPlannerData(data, 2, false, NULL);
00327 
00328     // Insert the edge connecting the two trees
00329     data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
00330 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines