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");
63  specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
64  specs_.approximateSolutions = false;
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 }
The exception type for ompl.
Definition: Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
Abstract definition of a goal region that can be sampled.
A shared pointer wrapper for ompl::base::Path.
An annotated vertex, adding information about its level in the quotient-space hiearchy,...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
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...
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...
const PlannerDataVertex & getVertex(unsigned int index) const
Retrieve a reference to the vertex object with the given index. If this vertex does not exist,...
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...
unsigned int addVertex(const PlannerDataVertex &st)
Adds the given vertex to the graph data. The vertex index is returned. Duplicates are not added....
Definition of an abstract state.
Definition: State.h:50
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
A graph on a quotient-space.
virtual void getPlannerData(ompl::base::PlannerData &data) const override
Return plannerdata structure, whereby each vertex is marked depending to which component it belongs (...
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal)
Shortest path on quotient-graph.
virtual double getImportance() const override
Importance of quotient-space depending on number of vertices in quotient-graph.
virtual void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
virtual void print(std::ostream &out) const override
Internal function implementing actual printing to stream.
void printConfiguration(const Configuration *) const
Print configuration to std::cout.
void init()
Initialization methods for the first iteration (adding start configuration and doing sanity checks)
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphBundle > Graph
A quotient-graph structure using boost::adjacency_list bundles.
void clearQuery() override
Clears internal datastructures of any query-specific information from the previous query....
virtual void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
A single quotient-space.
Definition: QuotientSpace.h:49
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66