Program Listing for File StateSpace.h

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

            virtual const double *getValueAddressAtIndex(const State *state, unsigned int index) const;

            virtual const std::vector<ValueLocation> &getValueLocations() const;

            virtual const std::map<std::string, ValueLocation> &getValueLocationsByName() const;

            virtual double *getValueAddressAtLocation(State *state, const ValueLocation &loc) const;

            virtual const double *getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;

            virtual double *getValueAddressAtName(State *state, const std::string &name) const;

            virtual 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;

            bool isMetricSpace() 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