BundleSpace.h

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  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 MPI-IS 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 */
 
#ifndef OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLE_
#define OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_BUNDLE_
 
#include <ompl/base/Planner.h>
#include <ompl/base/goals/GoalSampleableRegion.h>
#include <ompl/multilevel/datastructures/ProjectionFactory.h>
 
namespace ompl
{
    namespace multilevel
    {
 
        OMPL_CLASS_FORWARD(BundleSpaceMetric);
        OMPL_CLASS_FORWARD(Projection);
        OMPL_CLASS_FORWARD(BundleSpacePropagator);
 
        class BundleSpace : public ompl::base::Planner
        {
        private:
            using BaseT = ompl::base::Planner;
            using BaseT::si_;  // make it private.
            using BaseT::getSpaceInformation;
 
            // Note: use getBundle(), or getBase()
            // to access the SpaceInformationPtr
 
            ompl::base::PlannerStatus solve(const ompl::base::PlannerTerminationCondition &ptc) override final;
 
        public:
            BundleSpace(const ompl::base::SpaceInformationPtr &si, BundleSpace *baseSpace_ = nullptr);
 
            virtual ~BundleSpace();
 
            const ompl::base::SpaceInformationPtr &getBundle() const;
            const ompl::base::SpaceInformationPtr &getBase() const;
            ProjectionPtr getProjection() const;
 
            // projection.
            bool makeProjection();
 
            // projection)
            void setProjection(ProjectionPtr projection);
 
            virtual void setProblemDefinition(const ompl::base::ProblemDefinitionPtr &pdef) override;
 
            virtual void grow() = 0;
 
            virtual bool getSolution(ompl::base::PathPtr &solution) = 0;
 
            /* \brief Change the metric to be used on bundle space (default:
             * intrinsic bundle space metric) */
            virtual void setMetric(const std::string &sMetric) = 0;
 
            /* \brief Change the propagator to be used on bundle space (default:
             * intrinsic bundle space propagator) */
            virtual void setPropagator(const std::string &sPropagator) = 0;
 
            /* \brief Return a sample from the current datastructure on the
             * total space*/
            virtual void sampleFromDatastructure(ompl::base::State *xBase) = 0;
            /* \brief Return a sample from bundle space (using restriction
             * sampling) */
            virtual void sampleBundle(ompl::base::State *xRandom);
            /* \brief Same as sampleBundle, but return a valid state */
            bool sampleBundleValid(ompl::base::State *xRandom);
 
            /* \brief Check if there exists a solution */
            virtual bool hasSolution();
 
            virtual bool isInfeasible();
 
            virtual bool hasConverged();
 
            virtual void clear() override;
            virtual void setup() override;
 
            // allocate computational resources)
            virtual double getImportance() const = 0;
 
            ompl::base::State *allocIdentityStateBundle() const;
            ompl::base::State *allocIdentityStateBase() const;
            ompl::base::State *allocIdentityState(ompl::base::StateSpacePtr) const;
            void allocIdentityState(ompl::base::State *, ompl::base::StateSpacePtr) const;
 
            static void resetCounter();
 
            unsigned int getBaseDimension() const;
            unsigned int getBundleDimension() const;
            unsigned int getCoDimension() const;
 
            const ompl::base::StateSamplerPtr &getBundleSamplerPtr() const;
            const ompl::base::StateSamplerPtr &getBaseSamplerPtr() const;
 
            BundleSpace *getChild() const;
 
            void setChild(BundleSpace *child);
 
            BundleSpace *getParent() const;
 
            void setParent(BundleSpace *parent);
 
            bool hasParent() const;
 
            bool hasBaseSpace() const;
 
            unsigned int getLevel() const;
 
            void setLevel(unsigned int);
 
            void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const;
 
            void lift(const ompl::base::State *xBase, ompl::base::State *xBundle) const;
 
            ompl::base::OptimizationObjectivePtr getOptimizationObjectivePtr() const;
 
            friend std::ostream &operator<<(std::ostream &, const BundleSpace &);
 
            /* \brief Check if the current bundle space is dynamic*/
            bool isDynamic() const;
 
            /* \brief Get pointer to goal region on bundle space */
            base::GoalSampleableRegion *getGoalPtr() const;
 
        private:
            unsigned int level_{0};
 
            //\brief Being on the k-th bundle space, we denote as baseSpace the k-1-th
            // bundle space (because it locally acts as the base space for the current class)
            BundleSpace *childBundleSpace_{nullptr};
 
            //\brief Being on the k-th bundle space, we denote as parentSpace the k+1-th
            // bundle space
            BundleSpace *parentBundleSpace_{nullptr};
 
            /* \brief Total space of bundle */
            ompl::base::SpaceInformationPtr totalSpace_{nullptr};
 
            /* \brief Base space of bundle */
            ompl::base::SpaceInformationPtr baseSpace_{nullptr};
 
            /* \brief Pointer to uniform state sampler on total space */
            ompl::base::StateSamplerPtr Bundle_sampler_;
            ompl::base::ValidStateSamplerPtr Bundle_valid_sampler_;
 
            virtual bool findSection();
 
            ProjectionPtr projection_;
 
        protected:
            void checkBundleSpaceMeasure(std::string name, const ompl::base::StateSpacePtr space) const;
 
            void sanityChecks() const;
 
            virtual void print(std::ostream &out) const;
 
            ompl::base::State *xBaseTmp_{nullptr};
            ompl::base::State *xBundleTmp_{nullptr};
 
            static unsigned int counter_;
 
            unsigned int id_{0};
 
            bool hasSolution_{false};
 
            // least once
            bool firstRun_{true};
 
            bool isDynamic_{false};
 
            BundleSpaceMetricPtr metric_;
 
            BundleSpacePropagatorPtr propagator_;
        };
    }  // namespace multilevel
}  // namespace ompl
#endif