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