Loading...
Searching...
No Matches
PRM.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2011, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ioan Sucan, James D. Marble, Ryan Luna, Henning Kayser */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
38#define OMPL_GEOMETRIC_PLANNERS_PRM_PRM_
39
40#include "ompl/geometric/planners/PlannerIncludes.h"
41#include "ompl/datastructures/NearestNeighbors.h"
42#include <boost/graph/graph_traits.hpp>
43#include <boost/graph/adjacency_list.hpp>
44#include <boost/pending/disjoint_sets.hpp>
45#include <mutex>
46#include <utility>
47#include <vector>
48#include <map>
49
50namespace ompl
51{
52 namespace base
53 {
54 // Forward declare for use in implementation
55 OMPL_CLASS_FORWARD(OptimizationObjective);
56 }
57
58 namespace geometric
59 {
78
80 class PRM : public base::Planner
81 {
82 public:
84 {
85 using kind = boost::vertex_property_tag;
86 };
87
89 {
90 using kind = boost::vertex_property_tag;
91 };
92
94 {
95 using kind = boost::vertex_property_tag;
96 };
97
113 using Graph = boost::adjacency_list<
114 boost::vecS, boost::vecS, boost::undirectedS,
115 boost::property<
117 boost::property<
118 vertex_total_connection_attempts_t, unsigned long int,
119 boost::property<vertex_successful_connection_attempts_t, unsigned long int,
120 boost::property<boost::vertex_predecessor_t, unsigned long int,
121 boost::property<boost::vertex_rank_t, unsigned long int>>>>>,
122 boost::property<boost::edge_weight_t, base::Cost>>;
123
125 using Vertex = boost::graph_traits<Graph>::vertex_descriptor;
127 using Edge = boost::graph_traits<Graph>::edge_descriptor;
128
130 using RoadmapNeighbors = std::shared_ptr<NearestNeighbors<Vertex>>;
131
134 using ConnectionStrategy = std::function<const std::vector<Vertex> &(const Vertex)>;
135
141 using ConnectionFilter = std::function<bool(const Vertex &, const Vertex &)>;
142
144 PRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
145
147 PRM(const base::PlannerData &data, bool starStrategy = false);
148
149 ~PRM() override;
150
151 void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override;
152
166 void setConnectionStrategy(const ConnectionStrategy &connectionStrategy)
167 {
168 connectionStrategy_ = connectionStrategy;
170 }
171
173
177 void setMaxNearestNeighbors(unsigned int k);
178
183 unsigned int getMaxNearestNeighbors() const;
184
185
199 void setConnectionFilter(const ConnectionFilter &connectionFilter)
200 {
201 connectionFilter_ = connectionFilter;
202 }
203
204 void getPlannerData(base::PlannerData &data) const override;
205
210
214 void growRoadmap(double growTime);
215
220
224 void expandRoadmap(double expandTime);
225
230
245
250 void clearQuery() override;
251
252 void clear() override;
253
255 template <template <typename T> class NN>
257 {
258 if (nn_ && nn_->size() == 0)
259 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
260 clear();
261 nn_ = std::make_shared<NN<Vertex>>();
264 if (isSetup())
265 setup();
266 }
267
268 void setup() override;
269
270 const Graph &getRoadmap() const
271 {
272 return g_;
273 }
274
276 unsigned long int milestoneCount() const
277 {
278 return boost::num_vertices(g_);
279 }
280
282 unsigned long int edgeCount() const
283 {
284 return boost::num_edges(g_);
285 }
286
287 const RoadmapNeighbors &getNearestNeighbors()
288 {
289 return nn_;
290 }
291
292 protected:
294 void freeMemory();
295
299 Vertex addMilestone(base::State *state);
300
303 void uniteComponents(Vertex m1, Vertex m2);
304
307 bool sameComponent(Vertex m1, Vertex m2);
308
312 void growRoadmap(const base::PlannerTerminationCondition &ptc, base::State *workState);
313
317 void expandRoadmap(const base::PlannerTerminationCondition &ptc, std::vector<base::State *> &workStates);
318
320 void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution);
321
325 bool maybeConstructSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals,
326 base::PathPtr &solution);
327
330 ompl::base::Cost constructApproximateSolution(const std::vector<Vertex> &starts, const std::vector<Vertex> &goals, base::PathPtr &solution);
331
333 bool addedNewSolution() const;
334
337 base::PathPtr constructSolution(const Vertex &start, const Vertex &goal);
338
341 base::Cost costHeuristic(Vertex u, Vertex v) const;
342
345 double distanceFunction(const Vertex a, const Vertex b) const
346 {
347 return si_->distance(stateProperty_[a], stateProperty_[b]);
348 }
349
351 // Planner progress property functions
352 std::string getIterationCount() const
353 {
354 return std::to_string(iterations_);
355 }
356 std::string getBestCost() const
357 {
358 return std::to_string(bestCost_.value());
359 }
360 std::string getMilestoneCountString() const
361 {
362 return std::to_string(milestoneCount());
363 }
364 std::string getEdgeCountString() const
365 {
366 return std::to_string(edgeCount());
367 }
368
371
373 base::ValidStateSamplerPtr sampler_;
374
376 base::StateSamplerPtr simpleSampler_;
377
380
383
385 std::vector<Vertex> startM_;
386
388 std::vector<Vertex> goalM_;
389
391 boost::property_map<Graph, vertex_state_t>::type stateProperty_;
392
394 boost::property_map<Graph, vertex_total_connection_attempts_t>::type totalConnectionAttemptsProperty_;
395
397 boost::property_map<Graph, vertex_successful_connection_attempts_t>::type
399
401 boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
402
404 boost::disjoint_sets<boost::property_map<Graph, boost::vertex_rank_t>::type,
405 boost::property_map<Graph, boost::vertex_predecessor_t>::type> disjointSets_;
406
409
412
416
419
421 bool addedNewSolution_{false};
422
424 mutable std::mutex graphMutex_;
425
427 base::OptimizationObjectivePtr opt_;
428
430 // Planner progress properties
432 unsigned long int iterations_{0};
434 base::Cost bestCost_{std::numeric_limits<double>::quiet_NaN()};
435 };
436 }
437}
438
439#endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
double value() const
The value of the cost.
Definition Cost.h:56
Abstract definition of optimization objectives.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
bool isSetup() const
Check if setup() was called for this planner.
Definition Planner.cpp:113
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
Definition of an abstract state.
Definition State.h:50
base::Cost costHeuristic(Vertex u, Vertex v) const
Given two vertices, returns a heuristic on the cost of the path connecting them. This method wraps Op...
Definition PRM.cpp:750
std::mutex graphMutex_
Mutex to guard access to the Graph member (g_)
Definition PRM.h:424
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition PRM.h:370
ompl::base::Cost constructApproximateSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
(Assuming that there is always an approximate solution), finds an approximate solution.
Definition PRM.cpp:608
std::vector< Vertex > startM_
Array of start milestones.
Definition PRM.h:385
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. Grows a roadmap using constructRoadmap()....
Definition PRM.cpp:446
bool addedNewSolution() const
Returns the value of the addedNewSolution_ member.
Definition PRM.cpp:441
std::function< const std::vector< Vertex > &(const Vertex)> ConnectionStrategy
A function returning the milestones that should be attempted to connect to.
Definition PRM.h:134
boost::disjoint_sets< boost::property_map< Graph, boost::vertex_rank_t >::type, boost::property_map< Graph, boost::vertex_predecessor_t >::type > disjointSets_
Data structure that maintains the connected components.
Definition PRM.h:405
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition PRM.cpp:230
bool sameComponent(Vertex m1, Vertex m2)
Check if two milestones (m1 and m2) are part of the same connected component. This is not a const fun...
Definition PRM.cpp:603
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition PRM.h:427
bool addedNewSolution_
A flag indicating that a solution has been added during solve()
Definition PRM.h:421
base::Cost bestCost_
Best cost found so far by algorithm.
Definition PRM.h:434
void freeMemory()
Free all the memory allocated by the planner.
Definition PRM.cpp:251
base::StateSamplerPtr simpleSampler_
Sampler user for generating random in the state space.
Definition PRM.h:376
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition PRM.h:379
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition PRM.h:127
boost::graph_traits< Graph >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition PRM.h:125
PRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition PRM.cpp:72
void checkForSolution(const base::PlannerTerminationCondition &ptc, base::PathPtr &solution)
Definition PRM.cpp:379
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition PRM.h:256
void expandRoadmap(double expandTime)
Attempt to connect disjoint components in the roadmap using random bouncing motions (the PRM expansio...
Definition PRM.cpp:258
double distanceFunction(const Vertex a, const Vertex b) const
Compute distance between two milestones (this is simply distance between the states of the milestones...
Definition PRM.h:345
std::function< bool(const Vertex &, const Vertex &)> ConnectionFilter
A function that can reject connections.
Definition PRM.h:141
bool maybeConstructSolution(const std::vector< Vertex > &starts, const std::vector< Vertex > &goals, base::PathPtr &solution)
Check if there exists a solution, i.e., there exists a pair of milestones such that the first is in s...
Definition PRM.cpp:400
ConnectionStrategy connectionStrategy_
Function that returns the milestones to attempt connections with.
Definition PRM.h:408
base::PathPtr constructSolution(const Vertex &start, const Vertex &goal)
Given two milestones from the same connected component, construct a path connecting them and set it a...
Definition PRM.cpp:688
void setConnectionStrategy(const ConnectionStrategy &connectionStrategy)
Set the connection strategy function that specifies the milestones that connection attempts will be m...
Definition PRM.h:166
void uniteComponents(Vertex m1, Vertex m2)
Make two milestones (m1 and m2) be part of the same connected component. The component with fewer ele...
Definition PRM.cpp:598
void constructRoadmap(const base::PlannerTerminationCondition &ptc)
While the termination condition allows, this function will construct the roadmap (using growRoadmap()...
Definition PRM.cpp:530
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
Definition PRM.h:401
boost::property_map< Graph, vertex_successful_connection_attempts_t >::type successfulConnectionAttemptsProperty_
Access to the number of successful connection attempts for a vertex.
Definition PRM.h:398
unsigned long int iterations_
Number of iterations the algorithm performed.
Definition PRM.h:432
void growRoadmap(double growTime)
If the user desires, the roadmap can be improved for the given time (seconds). The solve() method wil...
Definition PRM.cpp:339
Graph g_
Connectivity graph.
Definition PRM.h:382
Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition PRM.cpp:562
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
Definition PRM.h:276
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, boost::property< vertex_state_t, base::State *, boost::property< vertex_total_connection_attempts_t, unsigned long int, boost::property< vertex_successful_connection_attempts_t, unsigned long int, boost::property< boost::vertex_predecessor_t, unsigned long int, boost::property< boost::vertex_rank_t, unsigned long int > > > > >, boost::property< boost::edge_weight_t, base::Cost > > Graph
The underlying roadmap graph.
Definition PRM.h:113
std::shared_ptr< NearestNeighbors< Vertex > > RoadmapNeighbors
A nearest neighbors data structure for roadmap vertices.
Definition PRM.h:130
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition PRM.cpp:155
bool userSetConnectionStrategy_
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
Definition PRM.h:415
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition PRM.cpp:237
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition PRM.cpp:193
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition PRM.cpp:721
std::vector< Vertex > goalM_
Array of goal milestones.
Definition PRM.h:388
unsigned int getMaxNearestNeighbors() const
return the maximum number of nearest neighbors to connect a sample to
Definition PRM.cpp:210
void setConnectionFilter(const ConnectionFilter &connectionFilter)
Set the function that can reject a milestone connection.
Definition PRM.h:199
base::ValidStateSamplerPtr sampler_
Sampler user for generating valid samples in the state space.
Definition PRM.h:373
void setDefaultConnectionStrategy()
Definition PRM.cpp:216
RNG rng_
Random number generator.
Definition PRM.h:418
unsigned long int edgeCount() const
Return the number of edges currently in the graph.
Definition PRM.h:282
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition PRM.h:391
boost::property_map< Graph, vertex_total_connection_attempts_t >::type totalConnectionAttemptsProperty_
Access to the number of total connection attempts for a vertex.
Definition PRM.h:394
ConnectionFilter connectionFilter_
Function that can reject a milestone connection.
Definition PRM.h:411
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()