Program Listing for File BundleSpaceGraph.h
↰ Return to documentation for file (src/ompl/multilevel/datastructures/BundleSpaceGraph.h)
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, University of Stuttgart.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the University of Stuttgart nor the names
* of its contributors may be used to endorse or promote products
* derived from this software without specific prior written
* permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Author: Andreas Orthey, Sohaib Akbar */
#ifndef OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLEGRAPH_
#define OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLEGRAPH_
#include <ompl/multilevel/datastructures/BundleSpace.h>
#include <ompl/multilevel/datastructures/pathrestriction/FindSectionTypes.h>
#include <limits>
#include <ompl/geometric/planners/PlannerIncludes.h>
#include <ompl/datastructures/NearestNeighbors.h>
#include <ompl/base/Cost.h>
#include <ompl/control/Control.h>
#include <ompl/datastructures/PDF.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/random.hpp>
// #include <boost/graph/subgraph.hpp> //Note: Everything would be nicer with
// subgraphs, but there are still some bugs in the boost impl which prevent
// us to use them here
#include <boost/graph/properties.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/variate_generator.hpp>
namespace ompl
{
namespace base
{
const double dInf = std::numeric_limits<double>::infinity();
}
namespace multilevel
{
OMPL_CLASS_FORWARD(BundleSpaceImportance);
OMPL_CLASS_FORWARD(BundleSpaceGraphSampler);
OMPL_CLASS_FORWARD(PathRestriction);
}
namespace geometric
{
OMPL_CLASS_FORWARD(PathSimplifier);
}
namespace multilevel
{
class BundleSpaceGraph : public BundleSpace
{
using BaseT = BundleSpace;
public:
using normalized_index_type = int;
class Configuration
{
public:
Configuration() = delete;
Configuration(const ompl::base::SpaceInformationPtr &si);
Configuration(const ompl::base::SpaceInformationPtr &si, const ompl::base::State *state_);
ompl::base::State *state{nullptr};
ompl::control::Control *control{nullptr};
unsigned int total_connection_attempts{0};
unsigned int successful_connection_attempts{0};
bool on_shortest_path{false};
void *pdf_element;
void setPDFElement(void *element_)
{
pdf_element = element_;
}
void *getPDFElement()
{
return pdf_element;
}
bool isStart{false};
bool isGoal{false};
Configuration *parent{nullptr};
base::Cost cost;
base::Cost lineCost;
std::vector<Configuration *> children;
normalized_index_type index{-1};
normalized_index_type representativeIndex{-1};
// boost::property<vertex_list_t, std::set<VertexIndexType>,
std::set<normalized_index_type> nonInterfaceIndexList;
// boost::property<vertex_interface_list_t, std::unordered_map<VertexIndexType,
// std::set<VertexIndexType>>>
std::unordered_map<normalized_index_type, std::set<normalized_index_type>> interfaceIndexList;
std::vector<Configuration *> reachableSet;
};
class EdgeInternalState
{
public:
EdgeInternalState() = default;
EdgeInternalState(ompl::base::Cost cost_) : cost(cost_){};
EdgeInternalState(const EdgeInternalState &eis)
{
cost = eis.cost;
}
EdgeInternalState& operator=(const EdgeInternalState &eis)
{
cost = eis.cost;
return *this;
}
void setWeight(double d)
{
cost = ompl::base::Cost(d);
}
ompl::base::Cost getCost()
{
return cost;
}
private:
ompl::base::Cost cost{+ompl::base::dInf};
};
struct GraphMetaData
{
std::string name{"BundleSpaceGraph"};
};
using Graph = boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, Configuration *,
EdgeInternalState, GraphMetaData>;
using BGT = boost::graph_traits<Graph>;
using Vertex = BGT::vertex_descriptor;
using Edge = BGT::edge_descriptor;
using VertexIndex = BGT::vertices_size_type;
using IEIterator = BGT::in_edge_iterator;
using OEIterator = BGT::out_edge_iterator;
using VertexParent = Vertex;
using VertexRank = VertexIndex;
using RoadmapNeighborsPtr = std::shared_ptr<NearestNeighbors<Configuration *>>;
using PDF = ompl::PDF<Configuration *>;
using PDF_Element = PDF::Element;
public:
BundleSpaceGraph(const ompl::base::SpaceInformationPtr &si, BundleSpace *parent = nullptr);
virtual ~BundleSpaceGraph();
/* \brief Number of vertices on boost graph */
virtual unsigned int getNumberOfVertices() const;
/* \brief Number of edges on boost graph */
virtual unsigned int getNumberOfEdges() const;
/* \brief A null vertex representing a non-existing vertex */
Vertex nullVertex() const;
/* \brief One iteration of the growing the graph */
void grow() override = 0;
/* \brief Sample from the graph (used in restriction sampling for
* parent bundle space) */
void sampleFromDatastructure(ompl::base::State *) override;
/* \brief as sampleBundle() but with goal bias */
virtual void sampleBundleGoalBias(ompl::base::State *xRandom);
/* \brief Check that there exist a path on graph */
bool getSolution(ompl::base::PathPtr &solution) override;
/* \brief Return best cost path on graph (as reference) */
virtual ompl::base::PathPtr &getSolutionPathByReference();
void getPlannerData(ompl::base::PlannerData &data) const override;
/* \brief Given graph, fill in the ompl::base::PlannerData structure */
void getPlannerDataGraph(ompl::base::PlannerData &data, const Graph &graph, const Vertex vStart) const;
double getImportance() const override;
virtual void init();
void setup() override;
void clear() override;
/* \brief Delete all vertices and their attached configurations */
virtual void clearVertices();
/* \brief Delete a configuration and free its state */
virtual void deleteConfiguration(Configuration *q);
/* \brief Create nearest neighbors structure */
template <template <typename T> class NN>
void setNearestNeighbors();
/* \brief Unite two components */
void uniteComponents(Vertex m1, Vertex m2);
/* \brief Check if both vertices are in the same component */
bool sameComponent(Vertex m1, Vertex m2);
std::map<Vertex, VertexRank> vrank;
std::map<Vertex, Vertex> vparent;
boost::disjoint_sets<boost::associative_property_map<std::map<Vertex, VertexRank>>,
boost::associative_property_map<std::map<Vertex, Vertex>>>
disjointSets_{boost::make_assoc_property_map(vrank), boost::make_assoc_property_map(vparent)};
/* \brief Get nearest configuration to configuration s */
virtual const Configuration *nearest(const Configuration *s) const;
/* \brief Set metric to be used for growing graph */
void setMetric(const std::string &sMetric) override;
void setPropagator(const std::string &sPropagator) override;
virtual void setImportance(const std::string &sImportance);
virtual void setGraphSampler(const std::string &sGraphSampler);
/* \brief Set strategy to solve the find section problem */
virtual void setFindSectionStrategy(FindSectionType type);
/* \brief Get current graph sampler */
BundleSpaceGraphSamplerPtr getGraphSampler();
base::Cost bestCost_{+base::dInf};
/* \brief Best cost path on graph as vertex representation */
std::vector<Vertex> shortestVertexPath_;
virtual Graph &getGraphNonConst();
virtual const Graph &getGraph() const;
const RoadmapNeighborsPtr &getRoadmapNeighborsPtr() const;
void print(std::ostream &out) const override;
void writeToGraphviz(std::string filename) const;
virtual void printConfiguration(const Configuration *) const;
void setGoalBias(double goalBias);
double getGoalBias() const;
void setRange(double distance);
double getRange() const;
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal);
ompl::base::PathPtr getPath(const Vertex &start, const Vertex &goal, Graph &graph);
virtual double distance(const Configuration *a, const Configuration *b) const;
virtual bool checkMotion(const Configuration *a, const Configuration *b) const;
bool connect(const Configuration *from, const Configuration *to);
Configuration *steerTowards(const Configuration *from, const Configuration *to);
Configuration *steerTowards_Range(const Configuration *from, Configuration *to);
Configuration *extendGraphTowards_Range(const Configuration *from, Configuration *to);
virtual void interpolate(const Configuration *a, const Configuration *b, Configuration *dest) const;
virtual Configuration *addBundleConfiguration(base::State *);
virtual Vertex addConfiguration(Configuration *q);
void addGoalConfiguration(Configuration *x);
virtual void addBundleEdge(const Configuration *a, const Configuration *b);
virtual const std::pair<Edge, bool> addEdge(const Vertex a, const Vertex b);
virtual Vertex getStartIndex() const;
virtual Vertex getGoalIndex() const;
virtual void setStartIndex(Vertex);
bool findSection() override;
protected:
ompl::base::PathPtr solutionPath_;
Vertex vStart_;
ompl::base::Cost costHeuristic(Vertex u, Vertex v) const;
RoadmapNeighborsPtr nearestDatastructure_;
Graph graph_;
unsigned int numVerticesWhenComputingSolutionPath_{0};
RNG rng_;
using RNGType = boost::minstd_rand;
RNGType rng_boost;
double graphLength_{0.0};
double maxDistance_{-1.0};
double goalBias_{.1};
Configuration *xRandom_{nullptr};
BundleSpaceImportancePtr importanceCalculator_{nullptr};
BundleSpaceGraphSamplerPtr graphSampler_{nullptr};
PathRestrictionPtr pathRestriction_{nullptr};
ompl::geometric::PathSimplifierPtr optimizer_;
Configuration *qStart_{nullptr};
Configuration *qGoal_{nullptr};
std::vector<Configuration *> startConfigurations_;
std::vector<Configuration *> goalConfigurations_;
};
} // namespace multilevel
} // namespace ompl
#endif