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