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