QuotientSpaceGraph.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18  * of its contributors may be used to endorse or promote products
19  * derived from this software without specific prior written
20  * permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Andreas Orthey */
37 #include "GoalVisitor.hpp"
38 #include <ompl/geometric/planners/quotientspace/datastructures/PlannerDataVertexAnnotated.h>
39 #include <ompl/geometric/planners/quotientspace/datastructures/QuotientSpaceGraph.h>
40 
41 #include <ompl/geometric/planners/prm/ConnectionStrategy.h>
42 #include <ompl/base/goals/GoalSampleableRegion.h>
43 #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
44 #include <ompl/datastructures/PDF.h>
45 #include <ompl/tools/config/SelfConfig.h>
46 #include <ompl/tools/config/MagicConstants.h>
47 #include <ompl/util/Exception.h>
48 
49 #include <boost/graph/astar_search.hpp>
50 #include <boost/graph/incremental_components.hpp>
51 #include <boost/property_map/vector_property_map.hpp>
52 #include <boost/property_map/transform_value_property_map.hpp>
53 #include <boost/foreach.hpp>
54 
55 #define foreach BOOST_FOREACH
56 
58 
59 ompl::geometric::QuotientSpaceGraph::QuotientSpaceGraph(const base::SpaceInformationPtr &si, QuotientSpace *parent_)
60  : BaseT(si, parent_)
61 {
62  setName("QuotientSpaceGraph");
65  specs_.optimizingPaths = false;
66 
67  if (!isSetup())
68  {
69  setup();
70  }
71 }
72 
73 ompl::geometric::QuotientSpaceGraph::~QuotientSpaceGraph()
74 {
75 }
76 
78 {
79  BaseT::setup();
80  if (!nearestDatastructure_)
81  {
82  nearestDatastructure_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Configuration *>(this));
83  nearestDatastructure_->setDistanceFunction(
84  [this](const Configuration *a, const Configuration *b) { return distance(a, b); });
85  }
86 
87  if (pdef_)
88  {
89  if (pdef_->hasOptimizationObjective())
90  {
91  opt_ = pdef_->getOptimizationObjective();
92  }
93  else
94  {
95  opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
96  }
97  firstRun_ = true;
98  setup_ = true;
99  }
100  else
101  {
102  setup_ = false;
103  }
104 }
105 
107 {
108  BaseT::clear();
109 
110  clearVertices();
111  clearQuery();
112  graphLength_ = 0;
113  bestCost_ = base::Cost(base::dInf);
114  setup_ = false;
115 }
116 
117 ompl::geometric::QuotientSpaceGraph::Configuration::Configuration(const base::SpaceInformationPtr &si)
118  : state(si->allocState())
119 {
120 }
121 ompl::geometric::QuotientSpaceGraph::Configuration::Configuration(const base::SpaceInformationPtr &si,
122  const base::State *state_)
123  : state(si->cloneState(state_))
124 {
125 }
126 
127 void ompl::geometric::QuotientSpaceGraph::deleteConfiguration(Configuration *q)
128 {
129  if (q != nullptr)
130  {
131  if (q->state != nullptr)
132  {
133  Q1->freeState(q->state);
134  }
135  delete q;
136  q = nullptr;
137  }
138 }
139 
140 void ompl::geometric::QuotientSpaceGraph::clearVertices()
141 {
142  if (nearestDatastructure_)
143  {
144  std::vector<Configuration *> configs;
145  nearestDatastructure_->list(configs);
146  for (auto &config : configs)
147  {
148  deleteConfiguration(config);
149  }
150  nearestDatastructure_->clear();
151  }
152  graph_.clear();
153 }
154 
156 {
157  pis_.restart();
158 }
159 
161 {
162  double N = (double)getNumberOfVertices();
163  return 1.0 / (N + 1);
164 }
165 
167 {
168  auto *goal = dynamic_cast<base::GoalSampleableRegion *>(pdef_->getGoal().get());
169  if (goal == nullptr)
170  {
171  OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
172  throw ompl::Exception("Unknown goal type");
173  }
174 
175  if (const base::State *st = pis_.nextStart())
176  {
177  if (st != nullptr)
178  {
179  qStart_ = new Configuration(Q1, st);
180  qStart_->isStart = true;
181  vStart_ = addConfiguration(qStart_);
182  }
183  }
184  if (qStart_ == nullptr)
185  {
186  OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
187  throw ompl::Exception("Invalid initial states.");
188  }
189 
190  if (const base::State *st = pis_.nextGoal())
191  {
192  if (st != nullptr)
193  {
194  qGoal_ = new Configuration(Q1, st);
195  qGoal_->isGoal = true;
196  }
197  }
198  if (qGoal_ == nullptr)
199  {
200  OMPL_ERROR("%s: There are no valid goal states!", getName().c_str());
201  throw ompl::Exception("Invalid goal states.");
202  }
203 }
204 
205 void ompl::geometric::QuotientSpaceGraph::uniteComponents(Vertex m1, Vertex m2)
206 {
207  disjointSets_.union_set(m1, m2);
208 }
209 
210 bool ompl::geometric::QuotientSpaceGraph::sameComponent(Vertex m1, Vertex m2)
211 {
212  return boost::same_component(m1, m2, disjointSets_);
213 }
214 
216 ompl::geometric::QuotientSpaceGraph::nearest(const Configuration *q) const
217 {
218  return nearestDatastructure_->nearest(const_cast<Configuration *>(q));
219 }
220 
221 ompl::geometric::QuotientSpaceGraph::Vertex ompl::geometric::QuotientSpaceGraph::addConfiguration(Configuration *q)
222 {
223  Vertex m = boost::add_vertex(q, graph_);
224  graph_[m]->total_connection_attempts = 1;
225  graph_[m]->successful_connection_attempts = 0;
226  disjointSets_.make_set(m);
227  nearestDatastructure_->add(q);
228  q->index = m;
229  return m;
230 }
231 
232 unsigned int ompl::geometric::QuotientSpaceGraph::getNumberOfVertices() const
233 {
234  return num_vertices(graph_);
235 }
236 
237 unsigned int ompl::geometric::QuotientSpaceGraph::getNumberOfEdges() const
238 {
239  return num_edges(graph_);
240 }
241 
242 const ompl::geometric::QuotientSpaceGraph::Graph &ompl::geometric::QuotientSpaceGraph::getGraph() const
243 {
244  return graph_;
245 }
246 
247 const ompl::geometric::QuotientSpaceGraph::RoadmapNeighborsPtr &
248 ompl::geometric::QuotientSpaceGraph::getRoadmapNeighborsPtr() const
249 {
250  return nearestDatastructure_;
251 }
252 
253 ompl::base::Cost ompl::geometric::QuotientSpaceGraph::costHeuristic(Vertex u, Vertex v) const
254 {
255  return opt_->motionCostHeuristic(graph_[u]->state, graph_[v]->state);
256 }
257 
258 template <template <typename T> class NN>
259 void ompl::geometric::QuotientSpaceGraph::setNearestNeighbors()
260 {
261  if (nearestDatastructure_ && nearestDatastructure_->size() == 0)
262  OMPL_WARN("Calling setNearestNeighbors will clear all states.");
263  clear();
264  nearestDatastructure_ = std::make_shared<NN<base::State *>>();
265  if (!isSetup())
266  {
267  setup();
268  }
269 }
270 
271 double ompl::geometric::QuotientSpaceGraph::distance(const Configuration *a, const Configuration *b) const
272 {
273  return si_->distance(a->state, b->state);
274 }
275 
276 void ompl::geometric::QuotientSpaceGraph::addEdge(const Vertex a, const Vertex b)
277 {
278  base::Cost weight = opt_->motionCost(graph_[a]->state, graph_[b]->state);
279  EdgeInternalState properties(weight);
280  boost::add_edge(a, b, properties, graph_);
281  uniteComponents(a, b);
282 }
283 
284 double ompl::geometric::QuotientSpaceGraph::getGraphLength() const
285 {
286  return graphLength_;
287 }
288 
289 bool ompl::geometric::QuotientSpaceGraph::getSolution(base::PathPtr &solution)
290 {
291  if (hasSolution_)
292  {
293  solutionPath_ = getPath(vStart_, vGoal_);
294  startGoalVertexPath_ = shortestVertexPath_;
295  solution = solutionPath_;
296  return true;
297  }
298  else
299  {
300  base::Goal *g = pdef_->getGoal().get();
301  bestCost_ = base::Cost(+base::dInf);
302  bool same_component = sameComponent(vStart_, vGoal_);
303 
304  if (same_component && g->isStartGoalPairValid(graph_[vGoal_]->state, graph_[vStart_]->state))
305  {
306  solutionPath_ = getPath(vStart_, vGoal_);
307  if (solutionPath_)
308  {
309  solution = solutionPath_;
310  hasSolution_ = true;
311  startGoalVertexPath_ = shortestVertexPath_;
312  return true;
313  }
314  }
315  }
316  return hasSolution_;
317 }
318 
320 {
321  std::vector<Vertex> prev(boost::num_vertices(graph_));
322  auto weight = boost::make_transform_value_property_map(std::mem_fn(&EdgeInternalState::getCost),
323  get(boost::edge_bundle, graph_));
324  try
325  {
326  boost::astar_search(graph_, start, [this, goal](const Vertex v) { return costHeuristic(v, goal); },
327  boost::predecessor_map(&prev[0])
328  .weight_map(weight)
329  .distance_compare([this](EdgeInternalState c1, EdgeInternalState c2) {
330  return opt_->isCostBetterThan(c1.getCost(), c2.getCost());
331  })
332  .distance_combine([this](EdgeInternalState c1, EdgeInternalState c2) {
333  return opt_->combineCosts(c1.getCost(), c2.getCost());
334  })
335  .distance_inf(opt_->infiniteCost())
336  .distance_zero(opt_->identityCost())
337  .visitor(AStarGoalVisitor<Vertex>(goal)));
338  }
339  catch (AStarFoundGoal &)
340  {
341  }
342 
343  auto p(std::make_shared<PathGeometric>(si_));
344  if (prev[goal] == goal)
345  {
346  return nullptr;
347  }
348 
349  std::vector<Vertex> vpath;
350  for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
351  {
352  graph_[pos]->on_shortest_path = true;
353  vpath.push_back(pos);
354  p->append(graph_[pos]->state);
355  }
356  graph_[start]->on_shortest_path = true;
357  vpath.push_back(start);
358  p->append(graph_[start]->state);
359 
360  shortestVertexPath_.clear();
361  shortestVertexPath_.insert(shortestVertexPath_.begin(), vpath.rbegin(), vpath.rend());
362  p->reverse();
363 
364  return p;
365 }
366 
367 bool ompl::geometric::QuotientSpaceGraph::sampleQuotient(base::State *q_random_graph)
368 {
369  // RANDOM EDGE SAMPLING
370  if (num_edges(graph_) == 0)
371  return false;
372 
373  Edge e = boost::random_edge(graph_, rng_boost);
374  while (!sameComponent(boost::source(e, graph_), vStart_))
375  {
376  e = boost::random_edge(graph_, rng_boost);
377  }
378 
379  double s = rng_.uniform01();
380 
381  const Vertex v1 = boost::source(e, graph_);
382  const Vertex v2 = boost::target(e, graph_);
383  const base::State *from = graph_[v1]->state;
384  const base::State *to = graph_[v2]->state;
385 
386  Q1->getStateSpace()->interpolate(from, to, s, q_random_graph);
387  return true;
388 }
389 
390 void ompl::geometric::QuotientSpaceGraph::print(std::ostream &out) const
391 {
392  BaseT::print(out);
393  out << std::endl
394  << " --[QuotientSpaceGraph has " << getNumberOfVertices() << " vertices and " << getNumberOfEdges()
395  << " edges.]" << std::endl;
396 }
397 
399 {
400  Q1->printState(q->state);
401 }
402 
404 {
405  std::vector<int> idxPathI;
406  QuotientSpace *pparent = getParent();
407  while (pparent != nullptr)
408  {
409  idxPathI.push_back(0);
410  pparent = pparent->getParent();
411  }
412  idxPathI.push_back(0);
413 
414  unsigned int startComponent = 0;
415  unsigned int goalComponent = 1;
416 
417  base::PlannerDataVertexAnnotated pstart(graph_[vStart_]->state, startComponent);
418  pstart.setPath(idxPathI);
419  data.addStartVertex(pstart);
420  if (hasSolution_)
421  {
422  goalComponent = 0;
423  base::PlannerDataVertexAnnotated pgoal(graph_[vGoal_]->state, goalComponent);
424  pgoal.setPath(idxPathI);
425  data.addGoalVertex(pgoal);
426  }
427 
428  unsigned int ctr = 0;
429  foreach (const Edge e, boost::edges(graph_))
430  {
431  const Vertex v1 = boost::source(e, graph_);
432  const Vertex v2 = boost::target(e, graph_);
433 
434  base::PlannerDataVertexAnnotated p1(graph_[v1]->state);
435  base::PlannerDataVertexAnnotated p2(graph_[v2]->state);
436  p1.setPath(idxPathI);
437  p2.setPath(idxPathI);
438 
439  unsigned int vi1 = data.addVertex(p1);
440  unsigned int vi2 = data.addVertex(p2);
441  data.addEdge(p1, p2);
442 
443  ctr++;
444 
445  unsigned int v1Component = const_cast<QuotientSpaceGraph *>(this)->disjointSets_.find_set(v1);
446  unsigned int v2Component = const_cast<QuotientSpaceGraph *>(this)->disjointSets_.find_set(v2);
449 
450  if (v1Component == startComponent || v2Component == startComponent)
451  {
452  v1a.setComponent(0);
453  v2a.setComponent(0);
454  }
455  else if (v1Component == goalComponent || v2Component == goalComponent)
456  {
457  v1a.setComponent(1);
458  v2a.setComponent(1);
459  }
460  else
461  {
462  v1a.setComponent(2);
463  v2a.setComponent(2);
464  }
465  }
466 }
A shared pointer wrapper for ompl::base::Path.
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition of an abstract state.
Definition: State.h:114
virtual double getImportance() const override
Importance of quotient-space depending on number of vertices in quotient-graph.
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:112
void clearQuery() override
Clears internal datastructures of any query-specific information from the previous query....
void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
void printConfiguration(const Configuration *) const
Print configuration to std::cout.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:239
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:493
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:270
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on quotient-graph.
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphBundle > Graph
A quotient-graph structure using boost::adjacency_list bundles.
An annotated vertex, adding information about its level in the quotient-space hiearchy,...
A graph on a quotient-space.
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:113
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
virtual void print(std::ostream &out) const override
Internal function implementing actual printing to stream.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:266
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Abstract definition of a goal region that can be sampled.
A single quotient-space.
The exception type for ompl.
Definition: Exception.h:79
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:260