StateSpace.h

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, 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: Ioan Sucan */
 
#ifndef OMPL_BASE_STATE_SPACE_
#define OMPL_BASE_STATE_SPACE_
 
#include "ompl/base/State.h"
#include "ompl/base/StateSpaceTypes.h"
#include "ompl/base/StateSampler.h"
#include "ompl/base/ProjectionEvaluator.h"
#include "ompl/base/GenericParam.h"
#include "ompl/util/Console.h"
#include "ompl/util/ClassForward.h"
#include <boost/concept_check.hpp>
#include <iostream>
#include <vector>
#include <string>
#include <map>
 
namespace ompl
{
    namespace base
    {
 
        OMPL_CLASS_FORWARD(StateSpace);
 
        class StateSpace
        {
        public:
            // non-copyable
            StateSpace(const StateSpace &) = delete;
            StateSpace &operator=(const StateSpace &) = delete;
 
            using StateType = ompl::base::State;
 
            StateSpace();
 
            virtual ~StateSpace();
 
            template <class T>
            T *as()
            {
                BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
 
                return static_cast<T *>(this);
            }
 
            template <class T>
            const T *as() const
            {
                BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
 
                return static_cast<const T *>(this);
            }
 
            struct SubstateLocation
            {
                std::vector<std::size_t> chain;
 
                const StateSpace *space;
            };
 
            struct ValueLocation
            {
                SubstateLocation stateLocation;
 
                std::size_t index;
            };
 
            enum SanityChecks
            {
 
                STATESPACE_DISTANCE_DIFFERENT_STATES = (1 << 1),
 
                STATESPACE_DISTANCE_SYMMETRIC = (1 << 2),
 
                STATESPACE_INTERPOLATION = (1 << 3),
 
                STATESPACE_TRIANGLE_INEQUALITY = (1 << 4),
 
                STATESPACE_DISTANCE_BOUND = (1 << 5),
 
                STATESPACE_RESPECT_BOUNDS = (1 << 6),
 
                STATESPACE_ENFORCE_BOUNDS_NO_OP = (1 << 7),
 
                STATESPACE_SERIALIZATION = (1 << 8)
            };
 
            virtual bool isCompound() const;
 
            virtual bool isDiscrete() const;
 
            virtual bool isHybrid() const;
 
            virtual bool isMetricSpace() const
            {
                return true;
            }
 
            virtual bool hasSymmetricDistance() const;
 
            virtual bool hasSymmetricInterpolate() const;
 
            const std::string &getName() const;
 
            void setName(const std::string &name);
 
            int getType() const
            {
                return type_;
            }
 
            bool includes(const StateSpacePtr &other) const;
 
            bool includes(const StateSpace *other) const;
 
            bool covers(const StateSpacePtr &other) const;
 
            bool covers(const StateSpace *other) const;
 
            ParamSet &params()
            {
                return params_;
            }
 
            const ParamSet &params() const
            {
                return params_;
            }
 
            virtual double getLongestValidSegmentFraction() const;
 
            virtual void setLongestValidSegmentFraction(double segmentFraction);
 
            virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
 
            virtual void setValidSegmentCountFactor(unsigned int factor);
 
            virtual unsigned int getValidSegmentCountFactor() const;
 
            virtual double getLongestValidSegmentLength() const;
 
            virtual void computeSignature(std::vector<int> &signature) const;
 
            virtual unsigned int getDimension() const = 0;
 
            virtual double getMaximumExtent() const = 0;
 
            virtual double getMeasure() const = 0;
 
            virtual void enforceBounds(State *state) const = 0;
 
            virtual bool satisfiesBounds(const State *state) const = 0;
 
            virtual void copyState(State *destination, const State *source) const = 0;
 
            State *cloneState(const State *source) const;
 
            virtual double distance(const State *state1, const State *state2) const = 0;
 
            virtual unsigned int getSerializationLength() const;
 
            virtual void serialize(void *serialization, const State *state) const;
 
            virtual void deserialize(State *state, const void *serialization) const;
 
            virtual bool equalStates(const State *state1, const State *state2) const = 0;
 
            virtual void interpolate(const State *from, const State *to, double t, State *state) const = 0;
 
            virtual StateSamplerPtr allocDefaultStateSampler() const = 0;
 
            virtual StateSamplerPtr allocStateSampler() const;
 
            void setStateSamplerAllocator(const StateSamplerAllocator &ssa);
 
            void clearStateSamplerAllocator();
 
            virtual State *allocState() const = 0;
 
            virtual void freeState(State *state) const = 0;
 
            virtual double *getValueAddressAtIndex(State *state, unsigned int index) const;
 
            const double *getValueAddressAtIndex(const State *state, unsigned int index) const;
 
            const std::vector<ValueLocation> &getValueLocations() const;
 
            const std::map<std::string, ValueLocation> &getValueLocationsByName() const;
 
            double *getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
 
            const double *getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
 
            double *getValueAddressAtName(State *state, const std::string &name) const;
 
            const double *getValueAddressAtName(const State *state, const std::string &name) const;
 
            virtual void copyToReals(std::vector<double> &reals, const State *source) const;
 
            virtual void copyFromReals(State *destination, const std::vector<double> &reals) const;
 
            void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
 
            void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
 
            virtual void registerProjections();
 
            ProjectionEvaluatorPtr getProjection(const std::string &name) const;
 
            ProjectionEvaluatorPtr getDefaultProjection() const;
 
            bool hasProjection(const std::string &name) const;
 
            bool hasDefaultProjection() const;
 
            const std::map<std::string, ProjectionEvaluatorPtr> &getRegisteredProjections() const;
 
            virtual void printState(const State *state, std::ostream &out = std::cout) const;
 
            virtual void printSettings(std::ostream &out) const;
 
            virtual void printProjections(std::ostream &out) const;
 
            virtual void sanityChecks(double zero, double eps, unsigned int flags) const;
 
            virtual void sanityChecks() const;
 
            void diagram(std::ostream &out) const;
 
            void list(std::ostream &out) const;
 
            static void Diagram(std::ostream &out);
 
            static void List(std::ostream &out);
 
            StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const;
 
            virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
 
            State *getSubstateAtLocation(State *state, const SubstateLocation &loc) const;
 
            const State *getSubstateAtLocation(const State *state, const SubstateLocation &loc) const;
 
            const std::map<std::string, SubstateLocation> &getSubstateLocationsByName() const;
 
            void getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const;
 
            void getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const;
 
            virtual void computeLocations();
 
            virtual void setup();
 
        protected:
            static const std::string DEFAULT_PROJECTION_NAME;
 
            int type_;
 
            StateSamplerAllocator ssa_;
 
            double maxExtent_;
 
            double longestValidSegmentFraction_;
 
            double longestValidSegment_;
 
            unsigned int longestValidSegmentCountFactor_;
 
            std::map<std::string, ProjectionEvaluatorPtr> projections_;
 
            ParamSet params_;
 
            std::vector<ValueLocation> valueLocationsInOrder_;
 
            std::map<std::string, ValueLocation> valueLocationsByName_;
 
            std::map<std::string, SubstateLocation> substateLocationsByName_;
 
        private:
            std::string name_;
        };
 
        class CompoundStateSpace : public StateSpace
        {
        public:
            using StateType = ompl::base::CompoundState;
 
            CompoundStateSpace();
 
            CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
 
            ~CompoundStateSpace() override = default;
 
            template <class T>
            T *as(const unsigned int index) const
            {
                BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
 
                return static_cast<T *>(getSubspace(index).get());
            }
 
            template <class T>
            T *as(const std::string &name) const
            {
                BOOST_CONCEPT_ASSERT((boost::Convertible<T *, StateSpace *>));
 
                return static_cast<T *>(getSubspace(name).get());
            }
 
            bool isCompound() const override;
 
            bool isHybrid() const override;
 
            void addSubspace(const StateSpacePtr &component, double weight);
 
            unsigned int getSubspaceCount() const;
 
            const StateSpacePtr &getSubspace(unsigned int index) const;
 
            const StateSpacePtr &getSubspace(const std::string &name) const;
 
            unsigned int getSubspaceIndex(const std::string &name) const;
 
            bool hasSubspace(const std::string &name) const;
 
            double getSubspaceWeight(unsigned int index) const;
 
            double getSubspaceWeight(const std::string &name) const;
 
            void setSubspaceWeight(unsigned int index, double weight);
 
            void setSubspaceWeight(const std::string &name, double weight);
 
            const std::vector<StateSpacePtr> &getSubspaces() const;
 
            const std::vector<double> &getSubspaceWeights() const;
 
            bool isLocked() const;
 
            void lock();
            StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const override;
 
            unsigned int getDimension() const override;
 
            double getMaximumExtent() const override;
 
            double getMeasure() const override;
 
            void enforceBounds(State *state) const override;
 
            bool satisfiesBounds(const State *state) const override;
 
            void copyState(State *destination, const State *source) const override;
 
            unsigned int getSerializationLength() const override;
 
            void serialize(void *serialization, const State *state) const override;
 
            void deserialize(State *state, const void *serialization) const override;
 
            double distance(const State *state1, const State *state2) const override;
 
            void setLongestValidSegmentFraction(double segmentFraction) override;
 
            unsigned int validSegmentCount(const State *state1, const State *state2) const override;
 
            bool equalStates(const State *state1, const State *state2) const override;
 
            void interpolate(const State *from, const State *to, double t, State *state) const override;
 
            StateSamplerPtr allocDefaultStateSampler() const override;
 
            State *allocState() const override;
 
            void freeState(State *state) const override;
 
            double *getValueAddressAtIndex(State *state, unsigned int index) const override;
 
            void printState(const State *state, std::ostream &out) const override;
 
            void printSettings(std::ostream &out) const override;
 
            void computeLocations() override;
 
            void setup() override;
 
        protected:
            void allocStateComponents(CompoundState *state) const;
 
            std::vector<StateSpacePtr> components_;
 
            unsigned int componentCount_{0u};
 
            std::vector<double> weights_;
 
            double weightSum_{0.0};
 
            bool locked_{false};
        };
 
        StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b);
 
        StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b);
 
        StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
 
        StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b);
        enum AdvancedStateCopyOperation
        {
            NO_DATA_COPIED = 0,
 
            SOME_DATA_COPIED = 1,
 
            ALL_DATA_COPIED = 2
        };
 
        AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
                                                 const State *source);
 
        AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
                                                 const State *source);
 
        AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS,
                                                 const State *source, const std::vector<std::string> &subspaces);
 
        AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest, const StateSpace *sourceS,
                                                 const State *source, const std::vector<std::string> &subspaces);
    }
}
 
#endif