All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
StateSpace.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef OMPL_BASE_STATE_SPACE_
00038 #define OMPL_BASE_STATE_SPACE_
00039 
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/StateSpaceTypes.h"
00042 #include "ompl/base/StateSampler.h"
00043 #include "ompl/base/ProjectionEvaluator.h"
00044 #include "ompl/base/GenericParam.h"
00045 #include "ompl/util/Console.h"
00046 #include "ompl/util/ClassForward.h"
00047 #include <boost/concept_check.hpp>
00048 #include <boost/noncopyable.hpp>
00049 #include <iostream>
00050 #include <vector>
00051 #include <string>
00052 #include <map>
00053 
00054 namespace ompl
00055 {
00056     namespace base
00057     {
00058 
00060 
00061         ClassForward(StateSpace);
00063 
00073         class StateSpace : private boost::noncopyable
00074         {
00075         public:
00076 
00078             typedef State StateType;
00079 
00081             StateSpace(void);
00082 
00083             virtual ~StateSpace(void);
00084 
00086             template<class T>
00087             T* as(void)
00088             {
00090                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00091 
00092                 return static_cast<T*>(this);
00093             }
00094 
00096             template<class T>
00097             const T* as(void) const
00098             {
00100                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00101 
00102                 return static_cast<const T*>(this);
00103             }
00104 
00106             struct ValueLocation
00107             {
00113                 std::vector<std::size_t> chain;
00114 
00116                 const base::StateSpace  *space;
00117 
00119                 std::size_t              index;
00120             };
00121 
00123             enum SanityChecks
00124                 {
00126                     STATESPACE_DISTANCE_TO_SELF          = (1<<0),
00127 
00129                     STATESPACE_EQUAL_TO_SELF             = (1<<1),
00130 
00132                     STATESPACE_DISTANCE_DIFFERENT_STATES = (1<<2),
00133 
00135                     STATESPACE_DISTANCE_SYMMETRIC        = (1<<3),
00136 
00138                     STATESPACE_INTERPOLATION             = (1<<4),
00139 
00141                     STATESPACE_TRIANGLE_INEQUALITY       = (1<<5),
00142 
00144                     STATESPACE_DISTANCE_BOUND            = (1<<6)
00145                 };
00146 
00151             virtual bool isCompound(void) const;
00152 
00159             virtual bool isDiscrete(void) const;
00160 
00162             virtual bool isHybrid(void) const;
00163 
00165             const std::string& getName(void) const;
00166 
00168             void setName(const std::string &name);
00169 
00173             int getType(void) const
00174             {
00175                 return type_;
00176             }
00177 
00179             bool includes(const StateSpacePtr &other) const;
00180 
00183             bool covers(const StateSpacePtr &other) const;
00184 
00186             ParamSet& params(void)
00187             {
00188                 return params_;
00189             }
00190 
00192             const ParamSet& params(void) const
00193             {
00194                 return params_;
00195             }
00196 
00202             virtual double getLongestValidSegmentFraction(void) const;
00203 
00214             virtual void setLongestValidSegmentFraction(double segmentFraction);
00215 
00217             virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00218 
00225             void setValidSegmentCountFactor(unsigned int factor);
00226 
00228             unsigned int getValidSegmentCountFactor(void) const;
00229 
00232             void computeSignature(std::vector<int> &signature) const;
00233 
00240             virtual unsigned int getDimension(void) const = 0;
00241 
00248             virtual double getMaximumExtent(void) const = 0;
00249 
00252             virtual void enforceBounds(State *state) const = 0;
00253 
00256             virtual bool satisfiesBounds(const State *state) const = 0;
00257 
00259             virtual void copyState(State *destination, const State *source) const = 0;
00260 
00263             virtual double distance(const State *state1, const State *state2) const = 0;
00264 
00266             virtual unsigned int getSerializationLength(void) const;
00267 
00269             virtual void serialize(void *serialization, const State *state) const;
00270 
00272             virtual void deserialize(State *state, const void *serialization) const;
00273 
00275             virtual bool equalStates(const State *state1, const State *state2) const = 0;
00276 
00280             virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
00281 
00283             virtual StateSamplerPtr allocDefaultStateSampler(void) const = 0;
00284 
00288             virtual StateSamplerPtr allocStateSampler(void) const;
00289 
00291             void setStateSamplerAllocator(const StateSamplerAllocator &ssa);
00292 
00294             void clearStateSamplerAllocator(void);
00295 
00297             virtual State* allocState(void) const = 0;
00298 
00300             virtual void freeState(State *state) const = 0;
00301 
00319             virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00320 
00322             const double* getValueAddressAtIndex(const State *state, const unsigned int index) const;
00323 
00326             const std::vector<ValueLocation>& getValueLocations(void) const;
00327 
00330             const std::map<std::string, ValueLocation>& getValueLocationsByName(void) const;
00331 
00333             double* getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
00334 
00336             const double* getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
00337 
00339             double* getValueAddressAtName(State *state, const std::string &name) const;
00340 
00342             const double* getValueAddressAtName(const State *state, const std::string &name) const;
00343 
00345             void copyToReals(std::vector<double> &reals, const State *source) const;
00346 
00348             void copyFromReals(State *destination, const std::vector<double> &reals) const;
00349 
00356             void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
00357 
00359             void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
00360 
00363             virtual void registerProjections(void);
00364 
00366             ProjectionEvaluatorPtr getProjection(const std::string &name) const;
00367 
00369             ProjectionEvaluatorPtr getDefaultProjection(void) const;
00370 
00372             bool hasProjection(const std::string &name) const;
00373 
00375             bool hasDefaultProjection(void) const;
00376 
00378             const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
00379 
00386             virtual void printState(const State *state, std::ostream &out) const;
00387 
00389             virtual void printSettings(std::ostream &out) const;
00390 
00392             virtual void printProjections(std::ostream &out) const;
00393 
00396             virtual void sanityChecks(void) const;
00397 
00400             virtual void sanityChecks(double zero, double eps, unsigned int flags) const;
00401 
00403             static void Diagram(std::ostream &out);
00404 
00406             static void List(std::ostream &out);
00407 
00412             virtual void computeLocations(void);
00413 
00422             virtual void setup(void);
00423 
00424         protected:
00425 
00427             static const std::string DEFAULT_PROJECTION_NAME;
00428 
00430             int                                           type_;
00431 
00433             StateSamplerAllocator                         ssa_;
00434 
00436             double                                        maxExtent_;
00437 
00439             double                                        longestValidSegmentFraction_;
00440 
00442             double                                        longestValidSegment_;
00443 
00445             unsigned int                                  longestValidSegmentCountFactor_;
00446 
00448             std::map<std::string, ProjectionEvaluatorPtr> projections_;
00449 
00451             ParamSet                                      params_;
00452 
00455             std::vector<ValueLocation>                    valueLocationsInOrder_;
00456 
00459             std::map<std::string, ValueLocation>          valueLocationsByName_;
00460 
00462             msg::Interface                                msg_;
00463 
00464         private:
00465 
00467             std::string                                   name_;
00468         };
00469 
00471         class CompoundStateSpace : public StateSpace
00472         {
00473         public:
00474 
00476             typedef CompoundState StateType;
00477 
00479             CompoundStateSpace(void);
00480 
00482             CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
00483 
00484             virtual ~CompoundStateSpace(void)
00485             {
00486             }
00487 
00489             template<class T>
00490             T* as(const unsigned int index) const
00491             {
00493                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00494 
00495                 return static_cast<T*>(getSubSpace(index).get());
00496             }
00497 
00499             template<class T>
00500             T* as(const std::string &name) const
00501             {
00503                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00504 
00505                 return static_cast<T*>(getSubSpace(name).get());
00506             }
00507 
00508             virtual bool isCompound(void) const;
00509 
00510             virtual bool isHybrid(void) const;
00511 
00517             void addSubSpace(const StateSpacePtr &component, double weight);
00518 
00520             unsigned int getSubSpaceCount(void) const;
00521 
00523             const StateSpacePtr& getSubSpace(const unsigned int index) const;
00524 
00526             const StateSpacePtr& getSubSpace(const std::string& name) const;
00527 
00529             unsigned int getSubSpaceIndex(const std::string& name) const;
00530 
00532             bool hasSubSpace(const std::string &name) const;
00533 
00535             double getSubSpaceWeight(const unsigned int index) const;
00536 
00538             double getSubSpaceWeight(const std::string &name) const;
00539 
00541             void setSubSpaceWeight(const unsigned int index, double weight);
00542 
00544             void setSubSpaceWeight(const std::string &name, double weight);
00545 
00547             const std::vector<StateSpacePtr>& getSubSpaces(void) const;
00548 
00550             const std::vector<double>& getSubSpaceWeights(void) const;
00551 
00555             bool isLocked(void) const;
00556 
00562             void lock(void);
00568             virtual unsigned int getDimension(void) const;
00569 
00570             virtual double getMaximumExtent(void) const;
00571 
00572             virtual void enforceBounds(State *state) const;
00573 
00574             virtual bool satisfiesBounds(const State *state) const;
00575 
00576             virtual void copyState(State *destination, const State *source) const;
00577 
00578             virtual unsigned int getSerializationLength(void) const;
00579 
00580             virtual void serialize(void *serialization, const State *state) const;
00581 
00582             virtual void deserialize(State *state, const void *serialization) const;
00583 
00584             virtual double distance(const State *state1, const State *state2) const;
00585 
00591             virtual void setLongestValidSegmentFraction(double segmentFraction);
00592 
00595             virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00596 
00597             virtual bool equalStates(const State *state1, const State *state2) const;
00598 
00599             virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00600 
00601             virtual StateSamplerPtr allocDefaultStateSampler(void) const;
00602 
00603             virtual State* allocState(void) const;
00604 
00605             virtual void freeState(State *state) const;
00606 
00607             virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00608 
00611             virtual void printState(const State *state, std::ostream &out) const;
00612 
00613             virtual void printSettings(std::ostream &out) const;
00614 
00615             virtual void computeLocations(void);
00616 
00617             virtual void setup(void);
00618 
00619         protected:
00620 
00622             void allocStateComponents(CompoundState *state) const;
00623 
00625             std::vector<StateSpacePtr>    components_;
00626 
00628             unsigned int                  componentCount_;
00629 
00631             std::vector<double>           weights_;
00632 
00634             double                        weightSum_;
00635 
00637             bool                          locked_;
00638 
00639         };
00640 
00653         StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b);
00654 
00661         StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b);
00662 
00665         StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
00666 
00669         StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b);
00680         int copyStateData(const StateSpacePtr &destS, State *dest,
00681                           const StateSpacePtr &sourceS, const State *source);
00682     }
00683 }
00684 
00685 #endif