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