Syclop.h

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Rice University
*  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 Rice University 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: Matt Maly */

#ifndef OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_
#define OMPL_CONTROL_PLANNERS_SYCLOP_SYCLOP_

#include <boost/graph/astar_search.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <unordered_map>
#include "ompl/control/planners/PlannerIncludes.h"
#include "ompl/control/planners/syclop/Decomposition.h"
#include "ompl/control/planners/syclop/GridDecomposition.h"
#include "ompl/datastructures/PDF.h"
#include "ompl/util/Hash.h"
#include <map>
#include <vector>

namespace ompl
{
    namespace control
    {
        class Syclop : public base::Planner
        {
        public:
            typedef std::function<double(int, int)> EdgeCostFactorFn;

            typedef std::function<void(int, int, std::vector<int>&)> LeadComputeFn;

            Syclop(const SpaceInformationPtr& si, const DecompositionPtr &d, const std::string& plannerName) : ompl::base::Planner(si, plannerName),
                numFreeVolSamples_(Defaults::NUM_FREEVOL_SAMPLES),
                probShortestPath_(Defaults::PROB_SHORTEST_PATH),
                probKeepAddingToAvail_(Defaults::PROB_KEEP_ADDING_TO_AVAIL),
                numRegionExpansions_(Defaults::NUM_REGION_EXPANSIONS),
                numTreeSelections_(Defaults::NUM_TREE_SELECTIONS),
                probAbandonLeadEarly_(Defaults::PROB_ABANDON_LEAD_EARLY),
                siC_(si.get()),
                decomp_(d),
                covGrid_(Defaults::COVGRID_LENGTH, decomp_),
                graphReady_(false),
                numMotions_(0)
            {
                specs_.approximateSolutions = true;

                Planner::declareParam<int>   ("free_volume_samples", this, &Syclop::setNumFreeVolumeSamples, &Syclop::getNumFreeVolumeSamples, "10000:10000:500000");
                Planner::declareParam<int>   ("num_region_expansions", this, &Syclop::setNumRegionExpansions, &Syclop::getNumRegionExpansions, "10:10:500");
                Planner::declareParam<int>   ("num_tree_expansions", this, &Syclop::setNumTreeExpansions, &Syclop::getNumTreeExpansions, "0:1:100");
                Planner::declareParam<double>("prob_abandon_lead_early", this, &Syclop::setProbAbandonLeadEarly, &Syclop::getProbAbandonLeadEarly, "0.:.05:1.");
                Planner::declareParam<double>("prob_add_available_regions", this, &Syclop::setProbAddingToAvailableRegions, &Syclop::getProbAddingToAvailableRegions, "0.:.05:1.");
                Planner::declareParam<double>("prob_shortest_path_lead", this, &Syclop::setProbShortestPathLead, &Syclop::getProbShortestPathLead, "0.:.05:1.");
            }

            virtual ~Syclop()
            {
            }


            virtual void setup();

            virtual void clear();

            virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);


            void setLeadComputeFn(const LeadComputeFn& compute);

            void addEdgeCostFactor(const EdgeCostFactorFn& factor);

            void clearEdgeCostFactors();

            int getNumFreeVolumeSamples () const
            {
                return numFreeVolSamples_;
            }

            void setNumFreeVolumeSamples (int numSamples)
            {
                numFreeVolSamples_ = numSamples;
            }

            double getProbShortestPathLead () const
            {
                return probShortestPath_;
            }

            void setProbShortestPathLead (double probability)
            {
                probShortestPath_ = probability;
            }

            double getProbAddingToAvailableRegions () const
            {
                return probKeepAddingToAvail_;
            }

            void setProbAddingToAvailableRegions (double probability)
            {
                probKeepAddingToAvail_ = probability;
            }

            int getNumRegionExpansions () const
            {
                return numRegionExpansions_;
            }

            void setNumRegionExpansions (int regionExpansions)
            {
                numRegionExpansions_ = regionExpansions;
            }

            int getNumTreeExpansions () const
            {
                return numTreeSelections_;
            }

            void setNumTreeExpansions (int treeExpansions)
            {
                numTreeSelections_ = treeExpansions;
            }

            double getProbAbandonLeadEarly () const
            {
                return probAbandonLeadEarly_;
            }

            void setProbAbandonLeadEarly (double probability)
            {
                probAbandonLeadEarly_ = probability;
            }

            struct Defaults
            {
                static const int    NUM_FREEVOL_SAMPLES         = 100000;
                static const int    COVGRID_LENGTH              = 128;
                static const int    NUM_REGION_EXPANSIONS       = 100;
                static const int    NUM_TREE_SELECTIONS         = 1;
                // C++ standard prohibits non-integral static const member initialization
                // These constants are set in Syclop.cpp.  C++11 standard changes this
                // with the constexpr keyword, but for compatibility this is not done.
                static const double PROB_ABANDON_LEAD_EARLY     /*= 0.25*/;
                static const double PROB_KEEP_ADDING_TO_AVAIL   /*= 0.50*/;
                static const double PROB_SHORTEST_PATH          /*= 0.95*/;
            };

        protected:

            #pragma pack(push, 4)  // push default byte alignment to stack and align the following structure to 4 byte boundary

            class Motion
            {
            public:
                Motion() : state(nullptr), control(nullptr), parent(nullptr), steps(0)
                {
                }
                Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), parent(nullptr), steps(0)
                {
                }
                virtual ~Motion()
                {
                }
                base::State *state;
                Control *control;
                const Motion *parent;
                unsigned int steps;
            };
            #pragma pack (pop)  // Restoring default byte alignment

            #pragma pack(push, 4) // push default byte alignment to stack and align the following structure to 4 byte boundary

            class Region
            {
            public:
                Region()
                {
                }
                virtual ~Region()
                {
                }

#if __cplusplus >= 201103L
                Region(const Region&) = default;
                Region& operator=(const Region&) = default;
                Region(Region&&) = default;
                Region& operator=(Region&&) = default;
#endif

                void clear()
                {
                    motions.clear();
                    covGridCells.clear();
                    pdfElem = nullptr;
                }

                std::set<int> covGridCells;
                std::vector<Motion*> motions;
                double volume;
                double freeVolume;
                double percentValidCells;
                double weight;
                double alpha;
                int index;
                unsigned int numSelections;
                PDF<int>::Element *pdfElem;
            };
            #pragma pack (pop)  // Restoring default byte alignment

            #pragma pack(push, 4)  // push default byte alignment to stack and align the following structure to 4 byte boundary

            class Adjacency
            {
            public:
                Adjacency()
                {
                }
                virtual ~Adjacency()
                {
                }
                void clear()
                {
                    covGridCells.clear();
                }
                std::set<int> covGridCells;
                const Region *source;
                const Region *target;
                double cost;
                int numLeadInclusions;
                int numSelections;
                bool empty;
            };
            #pragma pack (pop) // Restoring default byte alignment

            virtual Motion* addRoot(const base::State *s) = 0;

            virtual void selectAndExtend(Region &region, std::vector<Motion*>& newMotions) = 0;

            inline const Region& getRegionFromIndex(const int rid) const
            {
                return graph_[boost::vertex(rid,graph_)];
            }

            int numFreeVolSamples_;

            double probShortestPath_;

            double probKeepAddingToAvail_;

            int numRegionExpansions_;

            int numTreeSelections_;

            double probAbandonLeadEarly_;

            const SpaceInformation *siC_;

            DecompositionPtr decomp_;

            RNG rng_;

        private:

            struct HashRegionPair
            {
                size_t operator()(const std::pair<int,int> &p) const
                {
                    std::size_t hash = std::hash<int>()(p.first);
                    hash_combine(hash, p.second);
                    return hash;
                }
            };


            class CoverageGrid : public GridDecomposition
            {
            public:
                CoverageGrid(const int len, const DecompositionPtr& d) : GridDecomposition(len, d->getDimension(), d->getBounds()), decomp(d)
                {
                }

                virtual ~CoverageGrid()
                {
                }

                virtual void project(const base::State *s, std::vector<double>& coord) const
                {
                    decomp->project(s, coord);
                }

                virtual void sampleFullState(const base::StateSamplerPtr& /*sampler*/, const std::vector<double>& /*coord*/, base::State* /*s*/) const
                {
                }

            protected:
                const DecompositionPtr& decomp;
            };

            typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, Region, Adjacency> RegionGraph;
            typedef boost::graph_traits<RegionGraph>::vertex_descriptor Vertex;
            typedef boost::graph_traits<RegionGraph>::vertex_iterator VertexIter;
            typedef boost::property_map<RegionGraph, boost::vertex_index_t>::type VertexIndexMap;
            typedef boost::graph_traits<RegionGraph>::edge_iterator EdgeIter;

            friend class DecompositionHeuristic;

            class DecompositionHeuristic : public boost::astar_heuristic<RegionGraph, double>
            {
            public:
                DecompositionHeuristic(const Syclop *s, const Region &goal) : syclop(s), goalRegion(goal)
                {
                }

                double operator()(Vertex v)
                {
                    const Region &region = syclop->getRegionFromIndex(v);
                    return region.alpha*goalRegion.alpha;
                }
            private:
                const Syclop *syclop;
                const Region &goalRegion;
            };

            struct found_goal {};

            class GoalVisitor : public boost::default_astar_visitor
            {
            public:
                GoalVisitor(const int goal) : goalRegion(goal)
                {
                }
                void examine_vertex(Vertex v, const RegionGraph& /*g*/)
                {
                    if (static_cast<int>(v) == goalRegion)
                        throw found_goal();
                }
            private:
                const int goalRegion;
            };

            class RegionSet
            {
            public:
                int sampleUniform()
                {
                    if (empty())
                        return -1;
                    return regions.sample(rng.uniform01());
                }
                void insert(const int r)
                {
                    if (regToElem.count(r) == 0)
                        regToElem[r] = regions.add(r, 1);
                    else
                    {
                        PDF<int>::Element *elem = regToElem[r];
                        regions.update(elem, regions.getWeight(elem)+1);
                    }
                }
                void clear()
                {
                    regions.clear();
                    regToElem.clear();
                }
                std::size_t size() const
                {
                    return regions.size();
                }
                bool empty() const
                {
                    return regions.empty();
                }
            private:
                RNG rng;
                PDF<int> regions;
                std::unordered_map<int, PDF<int>::Element*> regToElem;
            };

            void initRegion(Region &r);

            void setupRegionEstimates();

            void updateRegion(Region &r);

            void initEdge(Adjacency &a, const Region *source, const Region *target);

            void setupEdgeEstimates();

            void updateEdge(Adjacency &a);

            bool updateCoverageEstimate(Region &r, const base::State *s);

            bool updateConnectionEstimate(const Region &c, const Region &d, const base::State *s);

            void buildGraph();

            void clearGraphDetails();

            int selectRegion();

            void computeAvailableRegions();

            void defaultComputeLead(int startRegion, int goalRegion, std::vector<int>& lead);

            double defaultEdgeCost(int r, int s);

            LeadComputeFn leadComputeFn;
            std::vector<int> lead_;
            PDF<int> availDist_;
            std::vector<EdgeCostFactorFn> edgeCostFactors_;
            CoverageGrid covGrid_;
            RegionGraph graph_;
            bool graphReady_;
            std::unordered_map<std::pair<int,int>, Adjacency*, HashRegionPair> regionsToEdge_;
            unsigned int numMotions_;
            RegionSet startRegions_;
            RegionSet goalRegions_;
        };
    }
}

#endif