ompl/geometric/planners/prm/src/PRM.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 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, James D. Marble, Ryan Luna */
00036 
00037 #include "ompl/geometric/planners/prm/PRM.h"
00038 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
00039 #include "ompl/base/goals/GoalSampleableRegion.h"
00040 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
00041 #include "ompl/datastructures/PDF.h"
00042 #include "ompl/tools/config/SelfConfig.h"
00043 #include "ompl/tools/config/MagicConstants.h"
00044 #include <boost/lambda/bind.hpp>
00045 #include <boost/graph/astar_search.hpp>
00046 #include <boost/graph/incremental_components.hpp>
00047 #include <boost/property_map/vector_property_map.hpp>
00048 #include <boost/foreach.hpp>
00049 #include <boost/thread.hpp>
00050 
00051 #include "GoalVisitor.hpp"
00052 
00053 #define foreach BOOST_FOREACH
00054 
00055 namespace ompl
00056 {
00057     namespace magic
00058     {
00059 
00062         static const unsigned int MAX_RANDOM_BOUNCE_STEPS   = 5;
00063 
00065         static const double ROADMAP_BUILD_TIME = 0.2;
00066 
00069         static const unsigned int DEFAULT_NEAREST_NEIGHBORS = 10;
00070     }
00071 }
00072 
00073 ompl::geometric::PRM::PRM(const base::SpaceInformationPtr &si, bool starStrategy) :
00074     base::Planner(si, "PRM"),
00075     starStrategy_(starStrategy),
00076     stateProperty_(boost::get(vertex_state_t(), g_)),
00077     totalConnectionAttemptsProperty_(boost::get(vertex_total_connection_attempts_t(), g_)),
00078     successfulConnectionAttemptsProperty_(boost::get(vertex_successful_connection_attempts_t(), g_)),
00079     weightProperty_(boost::get(boost::edge_weight, g_)),
00080     disjointSets_(boost::get(boost::vertex_rank, g_),
00081                   boost::get(boost::vertex_predecessor, g_)),
00082     userSetConnectionStrategy_(false),
00083     addedNewSolution_(false),
00084     iterations_(0),
00085     bestCost_(std::numeric_limits<double>::quiet_NaN())
00086 {
00087     specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00088     specs_.approximateSolutions = false;
00089     specs_.optimizingPaths = true;
00090 
00091     Planner::declareParam<unsigned int>("max_nearest_neighbors", this, &PRM::setMaxNearestNeighbors, std::string("8:1000"));
00092 
00093     addPlannerProgressProperty("iterations INTEGER",
00094                                boost::bind(&PRM::getIterationCount, this));
00095     addPlannerProgressProperty("best cost REAL",
00096                                boost::bind(&PRM::getBestCost, this));
00097     addPlannerProgressProperty("milestone count INTEGER",
00098                                boost::bind(&PRM::getMilestoneCountString, this));
00099     addPlannerProgressProperty("edge count INTEGER",
00100                                boost::bind(&PRM::getEdgeCountString, this));
00101 }
00102 
00103 ompl::geometric::PRM::~PRM()
00104 {
00105     freeMemory();
00106 }
00107 
00108 void ompl::geometric::PRM::setup()
00109 {
00110     Planner::setup();
00111     if (!nn_)
00112     {
00113         nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
00114         nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
00115     }
00116     if (!connectionStrategy_)
00117     {
00118         if (starStrategy_)
00119             connectionStrategy_ = KStarStrategy<Vertex>(boost::bind(&PRM::milestoneCount, this), nn_, si_->getStateDimension());
00120         else
00121             connectionStrategy_ = KStrategy<Vertex>(magic::DEFAULT_NEAREST_NEIGHBORS, nn_);
00122     }
00123     if (!connectionFilter_)
00124         connectionFilter_ = boost::lambda::constant(true);
00125 
00126     // Setup optimization objective
00127     //
00128     // If no optimization objective was specified, then default to
00129     // optimizing path length as computed by the distance() function
00130     // in the state space.
00131     if (pdef_)
00132     {
00133         if (pdef_->hasOptimizationObjective())
00134             opt_ = pdef_->getOptimizationObjective();
00135         else
00136         {
00137             opt_.reset(new base::PathLengthOptimizationObjective(si_));
00138             if (!starStrategy_)
00139                 opt_->setCostThreshold(opt_->infiniteCost());
00140         }
00141     }
00142     else
00143     {
00144         OMPL_INFORM("%s: problem definition is not set, deferring setup completion...", getName().c_str());
00145         setup_ = false;
00146     }
00147 }
00148 
00149 void ompl::geometric::PRM::setMaxNearestNeighbors(unsigned int k)
00150 {
00151     if (starStrategy_)
00152         throw Exception("Cannot set the maximum nearest neighbors for " + getName());
00153     if (!nn_)
00154     {
00155         nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Vertex>(si_->getStateSpace()));
00156         nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
00157     }
00158     if (!userSetConnectionStrategy_)
00159         connectionStrategy_.clear();
00160     if (isSetup())
00161         setup();
00162 }
00163 
00164 void ompl::geometric::PRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
00165 {
00166     Planner::setProblemDefinition(pdef);
00167     clearQuery();
00168 }
00169 
00170 void ompl::geometric::PRM::clearQuery()
00171 {
00172     startM_.clear();
00173     goalM_.clear();
00174     pis_.restart();
00175 }
00176 
00177 void ompl::geometric::PRM::clear()
00178 {
00179     Planner::clear();
00180     sampler_.reset();
00181     simpleSampler_.reset();
00182     freeMemory();
00183     if (nn_)
00184         nn_->clear();
00185     clearQuery();
00186 
00187     iterations_ = 0;
00188     bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
00189 }
00190 
00191 void ompl::geometric::PRM::freeMemory()
00192 {
00193     foreach (Vertex v, boost::vertices(g_))
00194         si_->freeState(stateProperty_[v]);
00195     g_.clear();
00196 }
00197 
00198 void ompl::geometric::PRM::expandRoadmap(double expandTime)
00199 {
00200     expandRoadmap(base::timedPlannerTerminationCondition(expandTime));
00201 }
00202 
00203 void ompl::geometric::PRM::expandRoadmap(const base::PlannerTerminationCondition &ptc)
00204 {
00205     if (!simpleSampler_)
00206         simpleSampler_ = si_->allocStateSampler();
00207 
00208     std::vector<base::State*> states(magic::MAX_RANDOM_BOUNCE_STEPS);
00209     si_->allocStates(states);
00210     expandRoadmap(ptc, states);
00211     si_->freeStates(states);
00212 }
00213 
00214 void ompl::geometric::PRM::expandRoadmap(const base::PlannerTerminationCondition &ptc,
00215                                          std::vector<base::State*> &workStates)
00216 {
00217     // construct a probability distribution over the vertices in the roadmap
00218     // as indicated in
00219     //  "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces"
00220     //        Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H. Overmars
00221 
00222     PDF<Vertex> pdf;
00223     foreach (Vertex v, boost::vertices(g_))
00224     {
00225         const unsigned long int t = totalConnectionAttemptsProperty_[v];
00226         pdf.add(v, (double)(t - successfulConnectionAttemptsProperty_[v]) / (double)t);
00227     }
00228 
00229     if (pdf.empty())
00230         return;
00231 
00232     while (ptc == false)
00233     {
00234         iterations_++;
00235         Vertex v = pdf.sample(rng_.uniform01());
00236         unsigned int s = si_->randomBounceMotion(simpleSampler_, stateProperty_[v], workStates.size(), workStates, false);
00237         if (s > 0)
00238         {
00239             s--;
00240             Vertex last = addMilestone(si_->cloneState(workStates[s]));
00241 
00242             graphMutex_.lock();
00243             for (unsigned int i = 0 ; i < s ; ++i)
00244             {
00245                 // add the vertex along the bouncing motion
00246                 Vertex m = boost::add_vertex(g_);
00247                 stateProperty_[m] = si_->cloneState(workStates[i]);
00248                 totalConnectionAttemptsProperty_[m] = 1;
00249                 successfulConnectionAttemptsProperty_[m] = 0;
00250                 disjointSets_.make_set(m);
00251 
00252                 // add the edge to the parent vertex
00253                 const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[m]);
00254                 const Graph::edge_property_type properties(weight);
00255                 boost::add_edge(v, m, properties, g_);
00256                 uniteComponents(v, m);
00257 
00258                 // add the vertex to the nearest neighbors data structure
00259                 nn_->add(m);
00260                 v = m;
00261             }
00262 
00263             // if there are intermediary states or the milestone has not been connected to the initially sampled vertex,
00264             // we add an edge
00265             if (s > 0 || !sameComponent(v, last))
00266             {
00267                 // add the edge to the parent vertex
00268                 const base::Cost weight = opt_->motionCost(stateProperty_[v], stateProperty_[last]);
00269                 const Graph::edge_property_type properties(weight);
00270                 boost::add_edge(v, last, properties, g_);
00271                 uniteComponents(v, last);
00272             }
00273             graphMutex_.unlock();
00274         }
00275     }
00276 }
00277 
00278 void ompl::geometric::PRM::growRoadmap(double growTime)
00279 {
00280     growRoadmap(base::timedPlannerTerminationCondition(growTime));
00281 }
00282 
00283 void ompl::geometric::PRM::growRoadmap(const base::PlannerTerminationCondition &ptc)
00284 {
00285     if (!isSetup())
00286         setup();
00287     if (!sampler_)
00288         sampler_ = si_->allocValidStateSampler();
00289 
00290     base::State *workState = si_->allocState();
00291     growRoadmap (ptc, workState);
00292     si_->freeState(workState);
00293 }
00294 
00295 void ompl::geometric::PRM::growRoadmap(const base::PlannerTerminationCondition &ptc,
00296                                        base::State *workState)
00297 {
00298     /* grow roadmap in the regular fashion -- sample valid states, add them to the roadmap, add valid connections */
00299     while (ptc == false)
00300     {
00301         iterations_++;
00302         // search for a valid state
00303         bool found = false;
00304         while (!found && ptc == false)
00305         {
00306             unsigned int attempts = 0;
00307             do
00308             {
00309                 found = sampler_->sample(workState);
00310                 attempts++;
00311             } while (attempts < magic::FIND_VALID_STATE_ATTEMPTS_WITHOUT_TERMINATION_CHECK && !found);
00312         }
00313         // add it as a milestone
00314         if (found)
00315             addMilestone(si_->cloneState(workState));
00316     }
00317 }
00318 
00319 void ompl::geometric::PRM::checkForSolution(const base::PlannerTerminationCondition &ptc,
00320                                             base::PathPtr &solution)
00321 {
00322     base::GoalSampleableRegion *goal = static_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00323     while (!ptc && !addedNewSolution_)
00324     {
00325         // Check for any new goal states
00326         if (goal->maxSampleCount() > goalM_.size())
00327         {
00328             const base::State *st = pis_.nextGoal();
00329             if (st)
00330                 goalM_.push_back(addMilestone(si_->cloneState(st)));
00331         }
00332 
00333         // Check for a solution
00334         addedNewSolution_ = maybeConstructSolution(startM_, goalM_, solution);
00335         // Sleep for 1ms
00336         if (!addedNewSolution_)
00337             boost::this_thread::sleep(boost::posix_time::milliseconds(1));
00338     }
00339 }
00340 
00341 bool ompl::geometric::PRM::maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution)
00342 {
00343     base::Goal *g = pdef_->getGoal().get();
00344     base::Cost sol_cost(opt_->infiniteCost());
00345     foreach (Vertex start, starts)
00346     {
00347         foreach (Vertex goal, goals)
00348         {
00349             // we lock because the connected components algorithm is incremental and may change disjointSets_
00350             graphMutex_.lock();
00351             bool same_component = sameComponent(start, goal);
00352             graphMutex_.unlock();
00353 
00354             if (same_component && g->isStartGoalPairValid(stateProperty_[goal], stateProperty_[start]))
00355             {
00356                 base::PathPtr p = constructSolution(start, goal);
00357                 if (p)
00358                 {
00359                     base::Cost pathCost = p->cost(opt_);
00360                     if (opt_->isCostBetterThan(pathCost, bestCost_))
00361                         bestCost_ = pathCost;
00362                     // Check if optimization objective is satisfied
00363                     if (opt_->isSatisfied(pathCost))
00364                     {
00365                         solution = p;
00366                         return true;
00367                     }
00368                     else if (opt_->isCostBetterThan(pathCost, sol_cost))
00369                     {
00370                         solution = p;
00371                         sol_cost = pathCost;
00372                     }
00373                 }
00374             }
00375         }
00376     }
00377 
00378     return false;
00379 }
00380 
00381 bool ompl::geometric::PRM::addedNewSolution() const
00382 {
00383     return addedNewSolution_;
00384 }
00385 
00386 ompl::base::PlannerStatus ompl::geometric::PRM::solve(const base::PlannerTerminationCondition &ptc)
00387 {
00388     checkValidity();
00389     base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00390 
00391     if (!goal)
00392     {
00393         OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
00394         return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
00395     }
00396 
00397     // Add the valid start states as milestones
00398     while (const base::State *st = pis_.nextStart())
00399         startM_.push_back(addMilestone(si_->cloneState(st)));
00400 
00401     if (startM_.size() == 0)
00402     {
00403         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00404         return base::PlannerStatus::INVALID_START;
00405     }
00406 
00407     if (!goal->couldSample())
00408     {
00409         OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
00410         return base::PlannerStatus::INVALID_GOAL;
00411     }
00412 
00413     // Ensure there is at least one valid goal state
00414     if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
00415     {
00416         const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
00417         if (st)
00418             goalM_.push_back(addMilestone(si_->cloneState(st)));
00419 
00420         if (goalM_.empty())
00421         {
00422             OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
00423             return base::PlannerStatus::INVALID_GOAL;
00424         }
00425     }
00426 
00427     unsigned long int nrStartStates = boost::num_vertices(g_);
00428     OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
00429 
00430     // Reset addedNewSolution_ member and create solution checking thread
00431     addedNewSolution_ = false;
00432     base::PathPtr sol;
00433     boost::thread slnThread(boost::bind(&PRM::checkForSolution, this, ptc, boost::ref(sol)));
00434 
00435     // construct new planner termination condition that fires when the given ptc is true, or a solution is found
00436     base::PlannerTerminationCondition ptcOrSolutionFound =
00437         base::plannerOrTerminationCondition(ptc, base::PlannerTerminationCondition(boost::bind(&PRM::addedNewSolution, this)));
00438 
00439     constructRoadmap(ptcOrSolutionFound);
00440 
00441     // Ensure slnThread is ceased before exiting solve
00442     slnThread.join();
00443 
00444     OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
00445 
00446     if (sol)
00447     {
00448         base::PlannerSolution psol(sol);
00449         psol.setPlannerName(getName());
00450         // if the solution was optimized, we mark it as such
00451         psol.setOptimized(opt_, bestCost_, addedNewSolution());
00452         pdef_->addSolutionPath(psol);
00453     }
00454 
00455     return sol ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00456 }
00457 
00458 void ompl::geometric::PRM::constructRoadmap(const base::PlannerTerminationCondition &ptc)
00459 {
00460     if (!isSetup())
00461         setup();
00462     if (!sampler_)
00463         sampler_ = si_->allocValidStateSampler();
00464     if (!simpleSampler_)
00465         simpleSampler_ = si_->allocStateSampler();
00466 
00467     std::vector<base::State*> xstates(magic::MAX_RANDOM_BOUNCE_STEPS);
00468     si_->allocStates(xstates);
00469     bool grow = true;
00470 
00471     bestCost_ = opt_->infiniteCost();
00472     while (ptc() == false)
00473     {
00474         // maintain a 2:1 ratio for growing/expansion of roadmap
00475         // call growRoadmap() twice as long for every call of expandRoadmap()
00476         if (grow)
00477             growRoadmap(base::plannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(2.0 * magic::ROADMAP_BUILD_TIME)), xstates[0]);
00478         else
00479             expandRoadmap(base::plannerOrTerminationCondition(ptc, base::timedPlannerTerminationCondition(magic::ROADMAP_BUILD_TIME)), xstates);
00480         grow = !grow;
00481     }
00482 
00483     si_->freeStates(xstates);
00484 }
00485 
00486 ompl::geometric::PRM::Vertex ompl::geometric::PRM::addMilestone(base::State *state)
00487 {
00488     boost::mutex::scoped_lock _(graphMutex_);
00489 
00490     Vertex m = boost::add_vertex(g_);
00491     stateProperty_[m] = state;
00492     totalConnectionAttemptsProperty_[m] = 1;
00493     successfulConnectionAttemptsProperty_[m] = 0;
00494 
00495     // Initialize to its own (dis)connected component.
00496     disjointSets_.make_set(m);
00497 
00498     // Which milestones will we attempt to connect to?
00499     const std::vector<Vertex>& neighbors = connectionStrategy_(m);
00500 
00501     foreach (Vertex n, neighbors)
00502         if (connectionFilter_(n, m))
00503         {
00504             totalConnectionAttemptsProperty_[m]++;
00505             totalConnectionAttemptsProperty_[n]++;
00506             if (si_->checkMotion(stateProperty_[n], stateProperty_[m]))
00507             {
00508                 successfulConnectionAttemptsProperty_[m]++;
00509                 successfulConnectionAttemptsProperty_[n]++;
00510                 const base::Cost weight = opt_->motionCost(stateProperty_[n], stateProperty_[m]);
00511                 const Graph::edge_property_type properties(weight);
00512                 boost::add_edge(n, m, properties, g_);
00513                 uniteComponents(n, m);
00514             }
00515         }
00516 
00517     nn_->add(m);
00518 
00519     return m;
00520 }
00521 
00522 void ompl::geometric::PRM::uniteComponents(Vertex m1, Vertex m2)
00523 {
00524     disjointSets_.union_set(m1, m2);
00525 }
00526 
00527 bool ompl::geometric::PRM::sameComponent(Vertex m1, Vertex m2)
00528 {
00529     return boost::same_component(m1, m2, disjointSets_);
00530 }
00531 
00532 ompl::base::PathPtr ompl::geometric::PRM::constructSolution(const Vertex &start, const Vertex &goal)
00533 {
00534     boost::mutex::scoped_lock _(graphMutex_);
00535     boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
00536 
00537     try
00538     {
00539         // Consider using a persistent distance_map if it's slow
00540         boost::astar_search(g_, start,
00541                             boost::bind(&PRM::costHeuristic, this, _1, goal),
00542                             boost::predecessor_map(prev).
00543                             distance_compare(boost::bind(&base::OptimizationObjective::
00544                                                          isCostBetterThan, opt_.get(), _1, _2)).
00545                             distance_combine(boost::bind(&base::OptimizationObjective::
00546                                                          combineCosts, opt_.get(), _1, _2)).
00547                             distance_inf(opt_->infiniteCost()).
00548                             distance_zero(opt_->identityCost()).
00549                             visitor(AStarGoalVisitor<Vertex>(goal)));
00550     }
00551     catch (AStarFoundGoal&)
00552     {
00553     }
00554 
00555     if (prev[goal] == goal)
00556         throw Exception(name_, "Could not find solution path");
00557 
00558     PathGeometric *p = new PathGeometric(si_);
00559     for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
00560         p->append(stateProperty_[pos]);
00561     p->append(stateProperty_[start]);
00562     p->reverse();
00563 
00564     return base::PathPtr(p);
00565 }
00566 
00567 void ompl::geometric::PRM::getPlannerData(base::PlannerData &data) const
00568 {
00569     Planner::getPlannerData(data);
00570 
00571     // Explicitly add start and goal states:
00572     for (size_t i = 0; i < startM_.size(); ++i)
00573         data.addStartVertex(base::PlannerDataVertex(stateProperty_[startM_[i]], const_cast<PRM*>(this)->disjointSets_.find_set(startM_[i])));
00574 
00575     for (size_t i = 0; i < goalM_.size(); ++i)
00576         data.addGoalVertex(base::PlannerDataVertex(stateProperty_[goalM_[i]], const_cast<PRM*>(this)->disjointSets_.find_set(goalM_[i])));
00577 
00578     // Adding edges and all other vertices simultaneously
00579     foreach(const Edge e, boost::edges(g_))
00580     {
00581         const Vertex v1 = boost::source(e, g_);
00582         const Vertex v2 = boost::target(e, g_);
00583         data.addEdge(base::PlannerDataVertex(stateProperty_[v1]),
00584                      base::PlannerDataVertex(stateProperty_[v2]));
00585 
00586         // Add the reverse edge, since we're constructing an undirected roadmap
00587         data.addEdge(base::PlannerDataVertex(stateProperty_[v2]),
00588                      base::PlannerDataVertex(stateProperty_[v1]));
00589 
00590         // Add tags for the newly added vertices
00591         data.tagState(stateProperty_[v1], const_cast<PRM*>(this)->disjointSets_.find_set(v1));
00592         data.tagState(stateProperty_[v2], const_cast<PRM*>(this)->disjointSets_.find_set(v2));
00593     }
00594 }
00595 
00596 ompl::base::Cost ompl::geometric::PRM::costHeuristic(Vertex u, Vertex v) const
00597 {
00598     return opt_->motionCostHeuristic(stateProperty_[u], stateProperty_[v]);
00599 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines