Loading...
Searching...
No Matches
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
59ompl::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
73ompl::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
117ompl::geometric::QuotientSpaceGraph::Configuration::Configuration(const base::SpaceInformationPtr &si)
118 : state(si->allocState())
119{
120}
121ompl::geometric::QuotientSpaceGraph::Configuration::Configuration(const base::SpaceInformationPtr &si,
122 const base::State *state_)
123 : state(si->cloneState(state_))
124{
125}
126
127void 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
140void 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
205void ompl::geometric::QuotientSpaceGraph::uniteComponents(Vertex m1, Vertex m2)
206{
207 disjointSets_.union_set(m1, m2);
208}
209
210bool ompl::geometric::QuotientSpaceGraph::sameComponent(Vertex m1, Vertex m2)
211{
212 return boost::same_component(m1, m2, disjointSets_);
213}
214
216ompl::geometric::QuotientSpaceGraph::nearest(const Configuration *q) const
217{
218 return nearestDatastructure_->nearest(const_cast<Configuration *>(q));
219}
220
221ompl::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
232unsigned int ompl::geometric::QuotientSpaceGraph::getNumberOfVertices() const
233{
234 return num_vertices(graph_);
235}
236
237unsigned int ompl::geometric::QuotientSpaceGraph::getNumberOfEdges() const
238{
239 return num_edges(graph_);
240}
241
242const ompl::geometric::QuotientSpaceGraph::Graph &ompl::geometric::QuotientSpaceGraph::getGraph() const
243{
244 return graph_;
245}
246
247const ompl::geometric::QuotientSpaceGraph::RoadmapNeighborsPtr &
248ompl::geometric::QuotientSpaceGraph::getRoadmapNeighborsPtr() const
249{
250 return nearestDatastructure_;
251}
252
253ompl::base::Cost ompl::geometric::QuotientSpaceGraph::costHeuristic(Vertex u, Vertex v) const
254{
255 return opt_->motionCostHeuristic(graph_[u]->state, graph_[v]->state);
256}
257
258template <template <typename T> class NN>
259void 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
271double ompl::geometric::QuotientSpaceGraph::distance(const Configuration *a, const Configuration *b) const
272{
273 return si_->distance(a->state, b->state);
274}
275
276void 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
284double ompl::geometric::QuotientSpaceGraph::getGraphLength() const
285{
286 return graphLength_;
287}
288
289bool 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
367bool 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
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,...
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)
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...
boost::adjacency_list< boost::vecS, boost::vecS, boost::undirectedS, Configuration *, EdgeInternalState, GraphBundle > Graph
A quotient-graph structure using boost::adjacency_list bundles.
A single quotient-space.
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