Program Listing for File BundleSpace.h

↰ Return to documentation for file (src/ompl/multilevel/datastructures/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