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 multilevel
    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