All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
BKPIECE1.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Rice University,
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 Rice University 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/BKPIECE1.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <cassert>
00041 
00042 void ompl::geometric::BKPIECE1::setup(void)
00043 {
00044     Planner::setup();
00045     SelfConfig sc(si_, getName());
00046     sc.configureProjectionEvaluator(projectionEvaluator_);
00047     sc.configurePlannerRange(maxDistance_);
00048 
00049     if (badScoreFactor_ < std::numeric_limits<double>::epsilon() || badScoreFactor_ > 1.0)
00050         throw Exception("Bad cell score factor must be in the range (0,1]");
00051     if (goodScoreFactor_ < std::numeric_limits<double>::epsilon() || goodScoreFactor_ > 1.0)
00052         throw Exception("Good cell score factor must be in the range (0,1]");
00053     if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00054         throw Exception("The minimum valid path fraction must be in the range (0,1]");
00055 
00056     dStart_.setDimension(projectionEvaluator_->getDimension());
00057     dGoal_.setDimension(projectionEvaluator_->getDimension());
00058 }
00059 
00060 bool ompl::geometric::BKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00061 {
00062     checkValidity();
00063     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00064 
00065     if (!goal)
00066     {
00067         msg_.error("Unknown type of goal (or goal undefined)");
00068         return false;
00069     }
00070 
00071     Discretization<Motion>::Coord xcoord;
00072 
00073     while (const base::State *st = pis_.nextStart())
00074     {
00075         Motion* motion = new Motion(si_);
00076         si_->copyState(motion->state, st);
00077         motion->root = motion->state;
00078         projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00079         dStart_.addMotion(motion, xcoord);
00080     }
00081 
00082     if (dStart_.getMotionCount() == 0)
00083     {
00084         msg_.error("Motion planning start tree could not be initialized!");
00085         return false;
00086     }
00087 
00088     if (!goal->canSample())
00089     {
00090         msg_.error("Insufficient states in sampleable goal region");
00091         return false;
00092     }
00093 
00094     if (!sampler_)
00095         sampler_ = si_->allocValidStateSampler();
00096 
00097     msg_.inform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
00098 
00099     std::vector<Motion*> solution;
00100     base::State *xstate = si_->allocState();
00101     bool      startTree = true;
00102 
00103     while (ptc() == false)
00104     {
00105         Discretization<Motion> &disc      = startTree ? dStart_ : dGoal_;
00106         startTree = !startTree;
00107         Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
00108         disc.countIteration();
00109 
00110         // if we have not sampled too many goals already
00111         if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
00112         {
00113             const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00114             if (st)
00115             {
00116                 Motion* motion = new Motion(si_);
00117                 si_->copyState(motion->state, st);
00118                 motion->root = motion->state;
00119                 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00120                 dGoal_.addMotion(motion, xcoord);
00121             }
00122             if (dGoal_.getMotionCount() == 0)
00123             {
00124                 msg_.error("Unable to sample any valid states for goal tree");
00125                 break;
00126             }
00127         }
00128 
00129         Discretization<Motion>::Cell *ecell    = NULL;
00130         Motion                       *existing = NULL;
00131         disc.selectMotion(existing, ecell);
00132         assert(existing);
00133         if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
00134         {
00135             std::pair<base::State*, double> fail(xstate, 0.0);
00136             bool keep = si_->checkMotion(existing->state, xstate, fail);
00137             if (!keep && fail.second > minValidPathFraction_)
00138                 keep = true;
00139 
00140             if (keep)
00141             {
00142                 /* create a motion */
00143                 Motion *motion = new Motion(si_);
00144                 si_->copyState(motion->state, xstate);
00145                 motion->root = existing->root;
00146                 motion->parent = existing;
00147 
00148                 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00149                 disc.addMotion(motion, xcoord);
00150 
00151                 Discretization<Motion>::Cell* cellC = otherDisc.getGrid().getCell(xcoord);
00152 
00153                 if (cellC && !cellC->data->motions.empty())
00154                 {
00155                     Motion* connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
00156 
00157                     if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root) &&
00158                         si_->checkMotion(motion->state, connectOther->state))
00159                     {
00160                         /* extract the motions and put them in solution vector */
00161 
00162                         std::vector<Motion*> mpath1;
00163                         while (motion != NULL)
00164                         {
00165                             mpath1.push_back(motion);
00166                             motion = motion->parent;
00167                         }
00168 
00169                         std::vector<Motion*> mpath2;
00170                         while (connectOther != NULL)
00171                         {
00172                             mpath2.push_back(connectOther);
00173                             connectOther = connectOther->parent;
00174                         }
00175 
00176                         if (startTree)
00177                             mpath1.swap(mpath2);
00178 
00179                         PathGeometric *path = new PathGeometric(si_);
00180                         path->states.reserve(mpath1.size() + mpath2.size());
00181                         for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00182                             path->states.push_back(si_->cloneState(mpath1[i]->state));
00183                         for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00184                             path->states.push_back(si_->cloneState(mpath2[i]->state));
00185 
00186                         goal->setDifference(0.0);
00187                         goal->setSolutionPath(base::PathPtr(path));
00188                         break;
00189                     }
00190                 }
00191 
00192                 ecell->data->score *= goodScoreFactor_;
00193             }
00194             else
00195                 ecell->data->score *= badScoreFactor_;
00196         }
00197         disc.updateCell(ecell);
00198     }
00199 
00200     si_->freeState(xstate);
00201 
00202     msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
00203                 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
00204                 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
00205                 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
00206 
00207     return goal->isAchieved();
00208 }
00209 
00210 void ompl::geometric::BKPIECE1::freeMotion(Motion *motion)
00211 {
00212     if (motion->state)
00213         si_->freeState(motion->state);
00214     delete motion;
00215 }
00216 
00217 void ompl::geometric::BKPIECE1::clear(void)
00218 {
00219     Planner::clear();
00220 
00221     sampler_.reset();
00222     dStart_.clear();
00223     dGoal_.clear();
00224 }
00225 
00226 void ompl::geometric::BKPIECE1::getPlannerData(base::PlannerData &data) const
00227 {
00228     Planner::getPlannerData(data);
00229     dStart_.getPlannerData(data, 1);
00230     dGoal_.getPlannerData(data, 2);
00231 }