All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/ompl/base/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 SubstateLocation
00107             {
00113                 std::vector<std::size_t> chain;
00114 
00116                 const StateSpace        *space;
00117             };
00118 
00120             struct ValueLocation
00121             {
00123                 SubstateLocation stateLocation;
00124 
00126                 std::size_t      index;
00127             };
00128 
00131             enum SanityChecks
00132                 {
00133 
00135                     STATESPACE_DISTANCE_DIFFERENT_STATES = (1<<1),
00136 
00138                     STATESPACE_DISTANCE_SYMMETRIC        = (1<<2),
00139 
00141                     STATESPACE_INTERPOLATION             = (1<<3),
00142 
00144                     STATESPACE_TRIANGLE_INEQUALITY       = (1<<4),
00145 
00147                     STATESPACE_DISTANCE_BOUND            = (1<<5),
00148 
00150                     STATESPACE_RESPECT_BOUNDS            = (1<<6),
00151 
00153                     STATESPACE_ENFORCE_BOUNDS_NO_OP      = (1<<7),
00154 
00156                     STATESPACE_SERIALIZATION             = (1<<8)
00157                 };
00158 
00163             virtual bool isCompound(void) const;
00164 
00171             virtual bool isDiscrete(void) const;
00172 
00174             virtual bool isHybrid(void) const;
00175 
00177             const std::string& getName(void) const;
00178 
00180             void setName(const std::string &name);
00181 
00185             int getType(void) const
00186             {
00187                 return type_;
00188             }
00189 
00191             bool includes(const StateSpacePtr &other) const;
00192 
00194             bool includes(const StateSpace *other) const;
00195 
00198             bool covers(const StateSpacePtr &other) const;
00199 
00202             bool covers(const StateSpace *other) const;
00203 
00205             ParamSet& params(void)
00206             {
00207                 return params_;
00208             }
00209 
00211             const ParamSet& params(void) const
00212             {
00213                 return params_;
00214             }
00215 
00221             virtual double getLongestValidSegmentFraction(void) const;
00222 
00233             virtual void setLongestValidSegmentFraction(double segmentFraction);
00234 
00236             virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00237 
00244             void setValidSegmentCountFactor(unsigned int factor);
00245 
00247             unsigned int getValidSegmentCountFactor(void) const;
00248 
00251             void computeSignature(std::vector<int> &signature) const;
00252 
00259             virtual unsigned int getDimension(void) const = 0;
00260 
00267             virtual double getMaximumExtent(void) const = 0;
00268 
00271             virtual void enforceBounds(State *state) const = 0;
00272 
00275             virtual bool satisfiesBounds(const State *state) const = 0;
00276 
00279             virtual void copyState(State *destination, const State *source) const = 0;
00280 
00283             virtual double distance(const State *state1, const State *state2) const = 0;
00284 
00286             virtual unsigned int getSerializationLength(void) const;
00287 
00289             virtual void serialize(void *serialization, const State *state) const;
00290 
00292             virtual void deserialize(State *state, const void *serialization) const;
00293 
00295             virtual bool equalStates(const State *state1, const State *state2) const = 0;
00296 
00300             virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
00301 
00303             virtual StateSamplerPtr allocDefaultStateSampler(void) const = 0;
00304 
00308             virtual StateSamplerPtr allocStateSampler(void) const;
00309 
00311             void setStateSamplerAllocator(const StateSamplerAllocator &ssa);
00312 
00314             void clearStateSamplerAllocator(void);
00315 
00317             virtual State* allocState(void) const = 0;
00318 
00320             virtual void freeState(State *state) const = 0;
00321 
00339             virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00340 
00342             const double* getValueAddressAtIndex(const State *state, const unsigned int index) const;
00343 
00346             const std::vector<ValueLocation>& getValueLocations(void) const;
00347 
00350             const std::map<std::string, ValueLocation>& getValueLocationsByName(void) const;
00351 
00353             double* getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
00354 
00356             const double* getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
00357 
00359             double* getValueAddressAtName(State *state, const std::string &name) const;
00360 
00362             const double* getValueAddressAtName(const State *state, const std::string &name) const;
00363 
00365             void copyToReals(std::vector<double> &reals, const State *source) const;
00366 
00368             void copyFromReals(State *destination, const std::vector<double> &reals) const;
00369 
00376             void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
00377 
00379             void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
00380 
00383             virtual void registerProjections(void);
00384 
00386             ProjectionEvaluatorPtr getProjection(const std::string &name) const;
00387 
00389             ProjectionEvaluatorPtr getDefaultProjection(void) const;
00390 
00392             bool hasProjection(const std::string &name) const;
00393 
00395             bool hasDefaultProjection(void) const;
00396 
00398             const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
00399 
00406             virtual void printState(const State *state, std::ostream &out) const;
00407 
00409             virtual void printSettings(std::ostream &out) const;
00410 
00412             virtual void printProjections(std::ostream &out) const;
00413 
00416             virtual void sanityChecks(double zero, double eps, unsigned int flags) const;
00417 
00420             virtual void sanityChecks(void) const;
00421 
00423             void diagram(std::ostream &out) const;
00424 
00426             void list(std::ostream &out) const;
00427 
00429             static void Diagram(std::ostream &out);
00430 
00432             static void List(std::ostream &out);
00433 
00440             StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const;
00441 
00443             virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
00444 
00446             State* getSubstateAtLocation(State *state, const SubstateLocation &loc) const;
00447 
00449             const State* getSubstateAtLocation(const State *state, const SubstateLocation &loc) const;
00450 
00452             const std::map<std::string, SubstateLocation>& getSubstateLocationsByName(void) const;
00453 
00456             void getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const;
00457 
00460             void getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const;
00461           
00464             virtual void computeLocations(void);
00465 
00476             virtual void setup(void);
00477 
00478         protected:
00479 
00481             static const std::string DEFAULT_PROJECTION_NAME;
00482 
00484             int                                           type_;
00485 
00487             StateSamplerAllocator                         ssa_;
00488 
00490             double                                        maxExtent_;
00491 
00493             double                                        longestValidSegmentFraction_;
00494 
00496             double                                        longestValidSegment_;
00497 
00499             unsigned int                                  longestValidSegmentCountFactor_;
00500 
00502             std::map<std::string, ProjectionEvaluatorPtr> projections_;
00503 
00505             ParamSet                                      params_;
00506 
00509             std::vector<ValueLocation>                    valueLocationsInOrder_;
00510 
00513             std::map<std::string, ValueLocation>          valueLocationsByName_;
00514 
00516             std::map<std::string, SubstateLocation>       substateLocationsByName_;
00517 
00518         private:
00519 
00521             std::string                                   name_;
00522         };
00523 
00525         class CompoundStateSpace : public StateSpace
00526         {
00527         public:
00528 
00530             typedef CompoundState StateType;
00531 
00533             CompoundStateSpace(void);
00534 
00536             CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
00537 
00538             virtual ~CompoundStateSpace(void)
00539             {
00540             }
00541 
00543             template<class T>
00544             T* as(const unsigned int index) const
00545             {
00547                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00548 
00549                 return static_cast<T*>(getSubspace(index).get());
00550             }
00551 
00553             template<class T>
00554             T* as(const std::string &name) const
00555             {
00557                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00558 
00559                 return static_cast<T*>(getSubspace(name).get());
00560             }
00561 
00562             virtual bool isCompound(void) const;
00563 
00564             virtual bool isHybrid(void) const;
00565 
00571             void addSubspace(const StateSpacePtr &component, double weight);
00572 
00574             unsigned int getSubspaceCount(void) const;
00575 
00577             const StateSpacePtr& getSubspace(const unsigned int index) const;
00578 
00580             const StateSpacePtr& getSubspace(const std::string& name) const;
00581 
00583             unsigned int getSubspaceIndex(const std::string& name) const;
00584 
00586             bool hasSubspace(const std::string &name) const;
00587 
00589             double getSubspaceWeight(const unsigned int index) const;
00590 
00592             double getSubspaceWeight(const std::string &name) const;
00593 
00595             void setSubspaceWeight(const unsigned int index, double weight);
00596 
00598             void setSubspaceWeight(const std::string &name, double weight);
00599 
00601             const std::vector<StateSpacePtr>& getSubspaces(void) const;
00602 
00604             const std::vector<double>& getSubspaceWeights(void) const;
00605 
00609             bool isLocked(void) const;
00610 
00616             void lock(void);
00622             virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
00623 
00629             virtual unsigned int getDimension(void) const;
00630 
00631             virtual double getMaximumExtent(void) const;
00632 
00633             virtual void enforceBounds(State *state) const;
00634 
00635             virtual bool satisfiesBounds(const State *state) const;
00636 
00637             virtual void copyState(State *destination, const State *source) const;
00638 
00639             virtual unsigned int getSerializationLength(void) const;
00640 
00641             virtual void serialize(void *serialization, const State *state) const;
00642 
00643             virtual void deserialize(State *state, const void *serialization) const;
00644 
00645             virtual double distance(const State *state1, const State *state2) const;
00646 
00652             virtual void setLongestValidSegmentFraction(double segmentFraction);
00653 
00656             virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00657 
00658             virtual bool equalStates(const State *state1, const State *state2) const;
00659 
00660             virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00661 
00662             virtual StateSamplerPtr allocDefaultStateSampler(void) const;
00663 
00664             virtual State* allocState(void) const;
00665 
00666             virtual void freeState(State *state) const;
00667 
00668             virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00669 
00672             virtual void printState(const State *state, std::ostream &out) const;
00673 
00674             virtual void printSettings(std::ostream &out) const;
00675 
00676             virtual void computeLocations(void);
00677 
00678             virtual void setup(void);
00679 
00680         protected:
00681 
00683             void allocStateComponents(CompoundState *state) const;
00684 
00686             std::vector<StateSpacePtr>    components_;
00687 
00689             unsigned int                  componentCount_;
00690 
00692             std::vector<double>           weights_;
00693 
00695             double                        weightSum_;
00696 
00698             bool                          locked_;
00699 
00700         };
00701 
00714         StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b);
00715 
00722         StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b);
00723 
00726         StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
00727 
00730         StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b);
00739         enum AdvancedStateCopyOperation
00740             {
00742                 NO_DATA_COPIED = 0,
00743 
00745                 SOME_DATA_COPIED = 1,
00746 
00748                 ALL_DATA_COPIED  = 2
00749             };
00750 
00758         AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
00759                                                  const StateSpacePtr &sourceS, const State *source);
00760 
00768         AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
00769                                                  const StateSpace *sourceS, const State *source);
00770 
00776         AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
00777                                                  const StateSpacePtr &sourceS, const State *source,
00778                                                  const std::vector<std::string> &subspaces);
00779 
00785         AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
00786                                                  const StateSpace *sourceS, const State *source,
00787                                                  const std::vector<std::string> &subspaces);
00790     }
00791 }
00792 
00793 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines