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