37#include "GoalVisitor.hpp"
38#include <ompl/geometric/planners/quotientspace/datastructures/PlannerDataVertexAnnotated.h>
39#include <ompl/geometric/planners/quotientspace/datastructures/QuotientSpaceGraph.h>
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>
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>
55#define foreach BOOST_FOREACH
59ompl::geometric::QuotientSpaceGraph::QuotientSpaceGraph(
const base::SpaceInformationPtr &si, QuotientSpace *parent_)
62 setName(
"QuotientSpaceGraph");
63 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
64 specs_.approximateSolutions =
false;
65 specs_.optimizingPaths =
false;
73ompl::geometric::QuotientSpaceGraph::~QuotientSpaceGraph()
80 if (!nearestDatastructure_)
82 nearestDatastructure_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Configuration *>(
this));
83 nearestDatastructure_->setDistanceFunction(
89 if (pdef_->hasOptimizationObjective())
91 opt_ = pdef_->getOptimizationObjective();
95 opt_ = std::make_shared<base::PathLengthOptimizationObjective>(si_);
117ompl::geometric::QuotientSpaceGraph::Configuration::Configuration(
const base::SpaceInformationPtr &si)
118 : state(si->allocState())
121ompl::geometric::QuotientSpaceGraph::Configuration::Configuration(
const base::SpaceInformationPtr &si,
123 : state(si->cloneState(state_))
127void ompl::geometric::QuotientSpaceGraph::deleteConfiguration(
Configuration *q)
131 if (q->state !=
nullptr)
133 Q1->freeState(q->state);
140void ompl::geometric::QuotientSpaceGraph::clearVertices()
142 if (nearestDatastructure_)
144 std::vector<Configuration *> configs;
145 nearestDatastructure_->list(configs);
146 for (
auto &config : configs)
148 deleteConfiguration(config);
150 nearestDatastructure_->clear();
162 double N = (double)getNumberOfVertices();
163 return 1.0 / (N + 1);
171 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
180 qStart_->isStart =
true;
181 vStart_ = addConfiguration(qStart_);
184 if (qStart_ ==
nullptr)
186 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
195 qGoal_->isGoal =
true;
198 if (qGoal_ ==
nullptr)
200 OMPL_ERROR(
"%s: There are no valid goal states!", getName().c_str());
205void ompl::geometric::QuotientSpaceGraph::uniteComponents(Vertex m1, Vertex m2)
207 disjointSets_.union_set(m1, m2);
210bool ompl::geometric::QuotientSpaceGraph::sameComponent(Vertex m1, Vertex m2)
212 return boost::same_component(m1, m2, disjointSets_);
216ompl::geometric::QuotientSpaceGraph::nearest(
const Configuration *q)
const
218 return nearestDatastructure_->nearest(
const_cast<Configuration *
>(q));
221ompl::geometric::QuotientSpaceGraph::Vertex ompl::geometric::QuotientSpaceGraph::addConfiguration(
Configuration *q)
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);
232unsigned int ompl::geometric::QuotientSpaceGraph::getNumberOfVertices()
const
234 return num_vertices(graph_);
237unsigned int ompl::geometric::QuotientSpaceGraph::getNumberOfEdges()
const
239 return num_edges(graph_);
247const ompl::geometric::QuotientSpaceGraph::RoadmapNeighborsPtr &
248ompl::geometric::QuotientSpaceGraph::getRoadmapNeighborsPtr()
const
250 return nearestDatastructure_;
253ompl::base::Cost ompl::geometric::QuotientSpaceGraph::costHeuristic(Vertex u, Vertex v)
const
255 return opt_->motionCostHeuristic(graph_[u]->state, graph_[v]->state);
258template <
template <
typename T>
class NN>
259void ompl::geometric::QuotientSpaceGraph::setNearestNeighbors()
261 if (nearestDatastructure_ && nearestDatastructure_->size() == 0)
262 OMPL_WARN(
"Calling setNearestNeighbors will clear all states.");
264 nearestDatastructure_ = std::make_shared<NN<base::State *>>();
273 return si_->distance(a->state, b->state);
276void ompl::geometric::QuotientSpaceGraph::addEdge(
const Vertex a,
const Vertex b)
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);
284double ompl::geometric::QuotientSpaceGraph::getGraphLength()
const
289bool ompl::geometric::QuotientSpaceGraph::getSolution(base::PathPtr &solution)
293 solutionPath_ = getPath(vStart_, vGoal_);
294 startGoalVertexPath_ = shortestVertexPath_;
295 solution = solutionPath_;
300 base::Goal *g = pdef_->getGoal().get();
301 bestCost_ = base::Cost(+base::dInf);
302 bool same_component = sameComponent(vStart_, vGoal_);
304 if (same_component && g->isStartGoalPairValid(graph_[vGoal_]->state, graph_[vStart_]->state))
306 solutionPath_ = getPath(vStart_, vGoal_);
309 solution = solutionPath_;
311 startGoalVertexPath_ = shortestVertexPath_;
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_));
326 boost::astar_search(graph_, start, [
this, goal](
const Vertex v) {
return costHeuristic(v, goal); },
327 boost::predecessor_map(&prev[0])
330 return opt_->isCostBetterThan(c1.getCost(), c2.getCost());
333 return opt_->combineCosts(c1.getCost(), c2.getCost());
335 .distance_inf(opt_->infiniteCost())
336 .distance_zero(opt_->identityCost())
337 .visitor(AStarGoalVisitor<Vertex>(goal)));
339 catch (AStarFoundGoal &)
343 auto p(std::make_shared<PathGeometric>(si_));
344 if (prev[goal] == goal)
349 std::vector<Vertex> vpath;
350 for (Vertex pos = goal; prev[pos] != pos; pos = prev[pos])
352 graph_[pos]->on_shortest_path =
true;
353 vpath.push_back(pos);
354 p->append(graph_[pos]->state);
356 graph_[start]->on_shortest_path =
true;
357 vpath.push_back(start);
358 p->append(graph_[start]->state);
360 shortestVertexPath_.clear();
361 shortestVertexPath_.insert(shortestVertexPath_.begin(), vpath.rbegin(), vpath.rend());
367bool ompl::geometric::QuotientSpaceGraph::sampleQuotient(
base::State *q_random_graph)
370 if (num_edges(graph_) == 0)
373 Edge e = boost::random_edge(graph_, rng_boost);
374 while (!sameComponent(boost::source(e, graph_), vStart_))
376 e = boost::random_edge(graph_, rng_boost);
379 double s = rng_.uniform01();
381 const Vertex v1 = boost::source(e, graph_);
382 const Vertex v2 = boost::target(e, graph_);
386 Q1->getStateSpace()->interpolate(from, to, s, q_random_graph);
394 <<
" --[QuotientSpaceGraph has " << getNumberOfVertices() <<
" vertices and " << getNumberOfEdges()
395 <<
" edges.]" << std::endl;
400 Q1->printState(q->state);
405 std::vector<int> idxPathI;
407 while (pparent !=
nullptr)
409 idxPathI.push_back(0);
412 idxPathI.push_back(0);
414 unsigned int startComponent = 0;
415 unsigned int goalComponent = 1;
418 pstart.setPath(idxPathI);
424 pgoal.setPath(idxPathI);
428 unsigned int ctr = 0;
429 foreach (
const Edge e, boost::edges(graph_))
431 const Vertex v1 = boost::source(e, graph_);
432 const Vertex v2 = boost::target(e, graph_);
436 p1.setPath(idxPathI);
437 p2.setPath(idxPathI);
445 unsigned int v1Component =
const_cast<QuotientSpaceGraph *
>(
this)->disjointSets_.find_set(v1);
446 unsigned int v2Component =
const_cast<QuotientSpaceGraph *
>(
this)->disjointSets_.find_set(v2);
450 if (v1Component == startComponent || v2Component == startComponent)
455 else if (v1Component == goalComponent || v2Component == goalComponent)
The exception type for ompl.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
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.
A configuration in quotient-space.
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
An edge in quotient-space.
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.
QuotientSpace * getParent() const
Parent is a more simplified quotient-space (higher in abstraction hierarchy)
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.