Loading...
Searching...
No Matches
LazyPRM.h
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2013, 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, Henning Kayser */
36
37#ifndef OMPL_GEOMETRIC_PLANNERS_PRM_LAZY_PRM_
38#define OMPL_GEOMETRIC_PLANNERS_PRM_LAZY_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 <utility>
45#include <vector>
46#include <map>
47
48namespace ompl
49{
50 namespace base
51 {
52 // Forward declare for use in implementation
53 OMPL_CLASS_FORWARD(OptimizationObjective);
54 }
55
56 namespace geometric
57 {
71
73 class LazyPRM : public base::Planner
74 {
75 public:
77 {
78 using kind = boost::vertex_property_tag;
79 };
80
82 {
83 using kind = boost::vertex_property_tag;
84 };
85
87 {
88 using kind = boost::vertex_property_tag;
89 };
90
92 {
93 using kind = boost::edge_property_tag;
94 };
95
97 using Vertex =
98 boost::adjacency_list_traits<boost::vecS, boost::listS, boost::undirectedS>::vertex_descriptor;
99
116 using Graph = boost::adjacency_list<
117 boost::vecS, boost::listS, boost::undirectedS,
118 boost::property<
120 boost::property<
121 boost::vertex_index_t, unsigned long int,
122 boost::property<vertex_flags_t, unsigned int,
123 boost::property<vertex_component_t, unsigned long int,
124 boost::property<boost::vertex_predecessor_t, Vertex,
125 boost::property<boost::vertex_rank_t,
126 unsigned long int>>>>>>,
127 boost::property<boost::edge_weight_t, base::Cost, boost::property<edge_flags_t, unsigned int>>>;
128
130 using Edge = boost::graph_traits<Graph>::edge_descriptor;
131
133 using RoadmapNeighbors = std::shared_ptr<NearestNeighbors<Vertex> >;
134
137 using ConnectionStrategy = std::function<const std::vector<Vertex> &(const Vertex)>;
138
144 using ConnectionFilter = std::function<bool (const Vertex &, const Vertex &)>;
145
147 LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy = false);
148
150 LazyPRM(const base::PlannerData &data, bool starStrategy = false);
151
152 ~LazyPRM() override;
153
155 void setRange(double distance);
156
158 double getRange() const
159 {
160 return maxDistance_;
161 }
162
164 template <template <typename T> class NN>
166 {
167 if (nn_ && nn_->size() == 0)
168 OMPL_WARN("Calling setNearestNeighbors will clear all states.");
169 clear();
170 nn_ = std::make_shared<NN<Vertex>>();
173 if (isSetup())
174 setup();
175 }
176
177 void setProblemDefinition(const base::ProblemDefinitionPtr &pdef) override;
178
192 void setConnectionStrategy(const ConnectionStrategy &connectionStrategy)
193 {
194 connectionStrategy_ = connectionStrategy;
196 }
197
199
203 void setMaxNearestNeighbors(unsigned int k);
204
218 void setConnectionFilter(const ConnectionFilter &connectionFilter)
219 {
220 connectionFilter_ = connectionFilter;
221 }
222
224 unsigned long int milestoneCount() const
225 {
226 return boost::num_vertices(g_);
227 }
228
230 unsigned long int edgeCount() const
231 {
232 return boost::num_edges(g_);
233 }
234
235 void getPlannerData(base::PlannerData &data) const override;
236
237 void setup() override;
238
239 void clear() override;
240
245 void clearQuery() override;
246
248 void clearValidity();
249
251
252 protected:
254 static const unsigned int VALIDITY_UNKNOWN = 0;
255
257 static const unsigned int VALIDITY_TRUE = 1;
258
260 // Planner progress property functions
261 std::string getIterationCount() const
262 {
263 return std::to_string(iterations_);
264 }
265 std::string getBestCost() const
266 {
267 return std::to_string(bestCost_.value());
268 }
269 std::string getMilestoneCountString() const
270 {
271 return std::to_string(milestoneCount());
272 }
273 std::string getEdgeCountString() const
274 {
275 return std::to_string(edgeCount());
276 }
277
279 void freeMemory();
280
284 Vertex addMilestone(base::State *state);
285
286 void uniteComponents(Vertex a, Vertex b);
287
288 void markComponent(Vertex v, unsigned long int newComponent);
289
292 long int solutionComponent(std::pair<std::size_t, std::size_t> *startGoalPair) const;
293
296 ompl::base::PathPtr constructSolution(const Vertex &start, const Vertex &goal);
297
300 double distanceFunction(const Vertex a, const Vertex b) const
301 {
302 return si_->distance(stateProperty_[a], stateProperty_[b]);
303 }
304
307 base::Cost costHeuristic(Vertex u, Vertex v) const;
308
311
314
317
321
323 double maxDistance_{0.};
324
326 base::StateSamplerPtr sampler_;
327
330
333
335 std::vector<Vertex> startM_;
336
338 std::vector<Vertex> goalM_;
339
341 boost::property_map<Graph, boost::vertex_index_t>::type indexProperty_;
342
344 boost::property_map<Graph, vertex_state_t>::type stateProperty_;
345
347 boost::property_map<Graph, boost::edge_weight_t>::type weightProperty_;
348
350 boost::property_map<Graph, vertex_component_t>::type vertexComponentProperty_;
351
353 boost::property_map<Graph, vertex_flags_t>::type vertexValidityProperty_;
354
356 boost::property_map<Graph, edge_flags_t>::type edgeValidityProperty_;
357
360 unsigned long int componentCount_{0};
361
363 std::map<unsigned long int, unsigned long int> componentSize_;
364
366 base::OptimizationObjectivePtr opt_;
367
368 base::Cost bestCost_{std::numeric_limits<double>::quiet_NaN()};
369
370 unsigned long int iterations_{0};
371 };
372 }
373}
374
375#endif
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
void clearQuery() override
Clear the query previously loaded from the ProblemDefinition. Subsequent calls to solve() will reuse ...
Definition LazyPRM.cpp:253
static const unsigned int VALIDITY_UNKNOWN
Flag indicating validity of an edge of a vertex.
Definition LazyPRM.h:254
void clearValidity()
change the validity flag of each node and edge to VALIDITY_UNKNOWN
Definition LazyPRM.cpp:260
void setRange(double distance)
Set the maximum length of a motion to be added to the roadmap.
Definition LazyPRM.cpp:203
std::vector< Vertex > startM_
Array of start milestones.
Definition LazyPRM.h:335
void setConnectionStrategy(const ConnectionStrategy &connectionStrategy)
Set the connection strategy function that specifies the milestones that connection attempts will be m...
Definition LazyPRM.h:192
unsigned long int componentCount_
Number of connected components created so far. This is used as an ID only, does not represent the act...
Definition LazyPRM.h:360
void freeMemory()
Free all the memory allocated by the planner.
Definition LazyPRM.cpp:281
bool userSetConnectionStrategy_
Flag indicating whether the employed connection strategy was set by the user (or defaults are assumed...
Definition LazyPRM.h:320
void setMaxNearestNeighbors(unsigned int k)
Convenience function that sets the connection strategy to the default one with k nearest neighbors.
Definition LazyPRM.cpp:212
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 LazyPRM.cpp:615
ompl::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 LazyPRM.cpp:488
std::function< const std::vector< Vertex > &(const Vertex)> ConnectionStrategy
A function returning the milestones that should be attempted to connect to.
Definition LazyPRM.h:137
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition LazyPRM.cpp:268
std::shared_ptr< NearestNeighbors< Vertex > > RoadmapNeighbors
A nearest neighbors data structure for roadmap vertices.
Definition LazyPRM.h:133
boost::property_map< Graph, vertex_component_t >::type vertexComponentProperty_
Access the connected component of a vertex.
Definition LazyPRM.h:350
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition LazyPRM.cpp:314
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
Definition LazyPRM.h:165
std::map< unsigned long int, unsigned long int > componentSize_
The number of elements in each component in the LazyPRM roadmap.
Definition LazyPRM.h:363
static const unsigned int VALIDITY_TRUE
Flag indicating validity of an edge of a vertex.
Definition LazyPRM.h:257
double maxDistance_
The maximum length of a motion to be added to a tree.
Definition LazyPRM.h:323
Vertex addMilestone(base::State *state)
Construct a milestone for a given state (state), store it in the nearest neighbors data structure and...
Definition LazyPRM.cpp:288
bool starStrategy_
Flag indicating whether the default connection strategy is the Star strategy.
Definition LazyPRM.h:310
Graph g_
Connectivity graph.
Definition LazyPRM.h:332
boost::property_map< Graph, boost::edge_weight_t >::type weightProperty_
Access to the weights of each Edge.
Definition LazyPRM.h:347
boost::adjacency_list< boost::vecS, boost::listS, boost::undirectedS, boost::property< vertex_state_t, base::State *, boost::property< boost::vertex_index_t, unsigned long int, boost::property< vertex_flags_t, unsigned int, boost::property< vertex_component_t, unsigned long int, boost::property< boost::vertex_predecessor_t, Vertex, boost::property< boost::vertex_rank_t, unsigned long int > > > > > >, boost::property< boost::edge_weight_t, base::Cost, boost::property< edge_flags_t, unsigned int > > > Graph
The underlying roadmap graph.
Definition LazyPRM.h:116
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 LazyPRM.cpp:620
boost::adjacency_list_traits< boost::vecS, boost::listS, boost::undirectedS >::vertex_descriptor Vertex
The type for a vertex in the roadmap.
Definition LazyPRM.h:97
void setConnectionFilter(const ConnectionFilter &connectionFilter)
Set the function that can reject a milestone connection.
Definition LazyPRM.h:218
unsigned long int edgeCount() const
Return the number of edges currently in the graph.
Definition LazyPRM.h:230
boost::property_map< Graph, vertex_flags_t >::type vertexValidityProperty_
Access the validity state of a vertex.
Definition LazyPRM.h:353
double getRange() const
Get the range the planner is using.
Definition LazyPRM.h:158
unsigned long int milestoneCount() const
Return the number of milestones currently in the graph.
Definition LazyPRM.h:224
LazyPRM(const base::SpaceInformationPtr &si, bool starStrategy=false)
Constructor.
Definition LazyPRM.cpp:67
RoadmapNeighbors nn_
Nearest neighbors data structure.
Definition LazyPRM.h:329
boost::property_map< Graph, vertex_state_t >::type stateProperty_
Access to the internal base::state at each Vertex.
Definition LazyPRM.h:344
std::vector< Vertex > goalM_
Array of goal milestones.
Definition LazyPRM.h:338
void setDefaultConnectionStrategy()
Definition LazyPRM.cpp:230
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 LazyPRM.h:300
std::function< bool(const Vertex &, const Vertex &)> ConnectionFilter
A function that can reject connections.
Definition LazyPRM.h:144
ConnectionStrategy connectionStrategy_
Function that returns the milestones to attempt connections with.
Definition LazyPRM.h:313
boost::graph_traits< Graph >::edge_descriptor Edge
The type for an edge in the roadmap.
Definition LazyPRM.h:130
long int solutionComponent(std::pair< std::size_t, std::size_t > *startGoalPair) const
Check if any pair of a start state and goal state are part of the same connected component....
Definition LazyPRM.cpp:470
boost::property_map< Graph, boost::vertex_index_t >::type indexProperty_
Access to the internal base::state at each Vertex.
Definition LazyPRM.h:341
boost::property_map< Graph, edge_flags_t >::type edgeValidityProperty_
Access the validity state of an edge.
Definition LazyPRM.h:356
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition LazyPRM.cpp:156
base::StateSamplerPtr sampler_
Sampler user for generating random in the state space.
Definition LazyPRM.h:326
base::OptimizationObjectivePtr opt_
Objective cost function for PRM graph edges.
Definition LazyPRM.h:366
ConnectionFilter connectionFilter_
Function that can reject a milestone connection.
Definition LazyPRM.h:316
#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()