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         OMPL_CLASS_FORWARD(StateSpace);
00063 
00073         class StateSpace : private boost::noncopyable
00074         {
00075         public:
00076 
00078             typedef State StateType;
00079 
00081             StateSpace();
00082 
00083             virtual ~StateSpace();
00084 
00086             template<class T>
00087             T* as()
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() 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() const;
00164 
00171             virtual bool isDiscrete() const;
00172 
00174             virtual bool isHybrid() const;
00175 
00178             virtual bool isMetricSpace() const
00179             {
00180                 return true;
00181             }
00182 
00184             virtual bool hasSymmetricDistance() const;
00185 
00187             virtual bool hasSymmetricInterpolate() const;
00188 
00190             const std::string& getName() const;
00191 
00193             void setName(const std::string &name);
00194 
00198             int getType() const
00199             {
00200                 return type_;
00201             }
00202 
00204             bool includes(const StateSpacePtr &other) const;
00205 
00207             bool includes(const StateSpace *other) const;
00208 
00211             bool covers(const StateSpacePtr &other) const;
00212 
00215             bool covers(const StateSpace *other) const;
00216 
00218             ParamSet& params()
00219             {
00220                 return params_;
00221             }
00222 
00224             const ParamSet& params() const
00225             {
00226                 return params_;
00227             }
00228 
00234             virtual double getLongestValidSegmentFraction() const;
00235 
00246             virtual void setLongestValidSegmentFraction(double segmentFraction);
00247 
00249             virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00250 
00257             void setValidSegmentCountFactor(unsigned int factor);
00258 
00260             unsigned int getValidSegmentCountFactor() const;
00261 
00263             double getLongestValidSegmentLength() const;
00264 
00267             void computeSignature(std::vector<int> &signature) const;
00268 
00275             virtual unsigned int getDimension() const = 0;
00276 
00283             virtual double getMaximumExtent() const = 0;
00284 
00286             virtual double getMeasure() const = 0;
00287 
00290             virtual void enforceBounds(State *state) const = 0;
00291 
00294             virtual bool satisfiesBounds(const State *state) const = 0;
00295 
00298             virtual void copyState(State *destination, const State *source) const = 0;
00299 
00302             virtual double distance(const State *state1, const State *state2) const = 0;
00303 
00305             virtual unsigned int getSerializationLength() const;
00306 
00308             virtual void serialize(void *serialization, const State *state) const;
00309 
00311             virtual void deserialize(State *state, const void *serialization) const;
00312 
00314             virtual bool equalStates(const State *state1, const State *state2) const = 0;
00315 
00319             virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
00320 
00322             virtual StateSamplerPtr allocDefaultStateSampler() const = 0;
00323 
00327             virtual StateSamplerPtr allocStateSampler() const;
00328 
00330             void setStateSamplerAllocator(const StateSamplerAllocator &ssa);
00331 
00333             void clearStateSamplerAllocator();
00334 
00336             virtual State* allocState() const = 0;
00337 
00339             virtual void freeState(State *state) const = 0;
00340 
00358             virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00359 
00361             const double* getValueAddressAtIndex(const State *state, const unsigned int index) const;
00362 
00365             const std::vector<ValueLocation>& getValueLocations() const;
00366 
00369             const std::map<std::string, ValueLocation>& getValueLocationsByName() const;
00370 
00372             double* getValueAddressAtLocation(State *state, const ValueLocation &loc) const;
00373 
00375             const double* getValueAddressAtLocation(const State *state, const ValueLocation &loc) const;
00376 
00378             double* getValueAddressAtName(State *state, const std::string &name) const;
00379 
00381             const double* getValueAddressAtName(const State *state, const std::string &name) const;
00382 
00384             void copyToReals(std::vector<double> &reals, const State *source) const;
00385 
00387             void copyFromReals(State *destination, const std::vector<double> &reals) const;
00388 
00395             void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
00396 
00398             void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
00399 
00402             virtual void registerProjections();
00403 
00405             ProjectionEvaluatorPtr getProjection(const std::string &name) const;
00406 
00408             ProjectionEvaluatorPtr getDefaultProjection() const;
00409 
00411             bool hasProjection(const std::string &name) const;
00412 
00414             bool hasDefaultProjection() const;
00415 
00417             const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections() const;
00418 
00425             virtual void printState(const State *state, std::ostream &out) const;
00426 
00428             virtual void printSettings(std::ostream &out) const;
00429 
00431             virtual void printProjections(std::ostream &out) const;
00432 
00435             virtual void sanityChecks(double zero, double eps, unsigned int flags) const;
00436 
00439             virtual void sanityChecks() const;
00440 
00442             void diagram(std::ostream &out) const;
00443 
00445             void list(std::ostream &out) const;
00446 
00448             static void Diagram(std::ostream &out);
00449 
00451             static void List(std::ostream &out);
00452 
00459             StateSamplerPtr allocSubspaceStateSampler(const StateSpacePtr &subspace) const;
00460 
00462             virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
00463 
00465             State* getSubstateAtLocation(State *state, const SubstateLocation &loc) const;
00466 
00468             const State* getSubstateAtLocation(const State *state, const SubstateLocation &loc) const;
00469 
00471             const std::map<std::string, SubstateLocation>& getSubstateLocationsByName() const;
00472 
00475             void getCommonSubspaces(const StateSpacePtr &other, std::vector<std::string> &subspaces) const;
00476 
00479             void getCommonSubspaces(const StateSpace *other, std::vector<std::string> &subspaces) const;
00480 
00483             virtual void computeLocations();
00484 
00495             virtual void setup();
00496 
00497         protected:
00498 
00500             static const std::string DEFAULT_PROJECTION_NAME;
00501 
00503             int                                           type_;
00504 
00506             StateSamplerAllocator                         ssa_;
00507 
00509             double                                        maxExtent_;
00510 
00512             double                                        longestValidSegmentFraction_;
00513 
00515             double                                        longestValidSegment_;
00516 
00518             unsigned int                                  longestValidSegmentCountFactor_;
00519 
00521             std::map<std::string, ProjectionEvaluatorPtr> projections_;
00522 
00524             ParamSet                                      params_;
00525 
00528             std::vector<ValueLocation>                    valueLocationsInOrder_;
00529 
00532             std::map<std::string, ValueLocation>          valueLocationsByName_;
00533 
00535             std::map<std::string, SubstateLocation>       substateLocationsByName_;
00536 
00537         private:
00538 
00540             std::string                                   name_;
00541         };
00542 
00544         class CompoundStateSpace : public StateSpace
00545         {
00546         public:
00547 
00549             typedef CompoundState StateType;
00550 
00552             CompoundStateSpace();
00553 
00555             CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
00556 
00557             virtual ~CompoundStateSpace()
00558             {
00559             }
00560 
00562             template<class T>
00563             T* as(const unsigned int index) const
00564             {
00566                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00567 
00568                 return static_cast<T*>(getSubspace(index).get());
00569             }
00570 
00572             template<class T>
00573             T* as(const std::string &name) const
00574             {
00576                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00577 
00578                 return static_cast<T*>(getSubspace(name).get());
00579             }
00580 
00581             virtual bool isCompound() const;
00582 
00583             virtual bool isHybrid() const;
00584 
00590             void addSubspace(const StateSpacePtr &component, double weight);
00591 
00593             unsigned int getSubspaceCount() const;
00594 
00596             const StateSpacePtr& getSubspace(const unsigned int index) const;
00597 
00599             const StateSpacePtr& getSubspace(const std::string& name) const;
00600 
00602             unsigned int getSubspaceIndex(const std::string& name) const;
00603 
00605             bool hasSubspace(const std::string &name) const;
00606 
00608             double getSubspaceWeight(const unsigned int index) const;
00609 
00611             double getSubspaceWeight(const std::string &name) const;
00612 
00614             void setSubspaceWeight(const unsigned int index, double weight);
00615 
00617             void setSubspaceWeight(const std::string &name, double weight);
00618 
00620             const std::vector<StateSpacePtr>& getSubspaces() const;
00621 
00623             const std::vector<double>& getSubspaceWeights() const;
00624 
00628             bool isLocked() const;
00629 
00635             void lock();
00641             virtual StateSamplerPtr allocSubspaceStateSampler(const StateSpace *subspace) const;
00642 
00648             virtual unsigned int getDimension() const;
00649 
00650             virtual double getMaximumExtent() const;
00651 
00652             virtual double getMeasure() const;
00653 
00654             virtual void enforceBounds(State *state) const;
00655 
00656             virtual bool satisfiesBounds(const State *state) const;
00657 
00658             virtual void copyState(State *destination, const State *source) const;
00659 
00660             virtual unsigned int getSerializationLength() const;
00661 
00662             virtual void serialize(void *serialization, const State *state) const;
00663 
00664             virtual void deserialize(State *state, const void *serialization) const;
00665 
00666             virtual double distance(const State *state1, const State *state2) const;
00667 
00673             virtual void setLongestValidSegmentFraction(double segmentFraction);
00674 
00677             virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00678 
00679             virtual bool equalStates(const State *state1, const State *state2) const;
00680 
00681             virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00682 
00683             virtual StateSamplerPtr allocDefaultStateSampler() const;
00684 
00685             virtual State* allocState() const;
00686 
00687             virtual void freeState(State *state) const;
00688 
00689             virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00690 
00693             virtual void printState(const State *state, std::ostream &out) const;
00694 
00695             virtual void printSettings(std::ostream &out) const;
00696 
00697             virtual void computeLocations();
00698 
00699             virtual void setup();
00700 
00701         protected:
00702 
00704             void allocStateComponents(CompoundState *state) const;
00705 
00707             std::vector<StateSpacePtr>    components_;
00708 
00710             unsigned int                  componentCount_;
00711 
00713             std::vector<double>           weights_;
00714 
00716             double                        weightSum_;
00717 
00719             bool                          locked_;
00720 
00721         };
00722 
00735         StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b);
00736 
00743         StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b);
00744 
00747         StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
00748 
00751         StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b);
00760         enum AdvancedStateCopyOperation
00761             {
00763                 NO_DATA_COPIED = 0,
00764 
00766                 SOME_DATA_COPIED = 1,
00767 
00769                 ALL_DATA_COPIED  = 2
00770             };
00771 
00779         AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
00780                                                  const StateSpacePtr &sourceS, const State *source);
00781 
00789         AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
00790                                                  const StateSpace *sourceS, const State *source);
00791 
00797         AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest,
00798                                                  const StateSpacePtr &sourceS, const State *source,
00799                                                  const std::vector<std::string> &subspaces);
00800 
00806         AdvancedStateCopyOperation copyStateData(const StateSpace *destS, State *dest,
00807                                                  const StateSpace *sourceS, const State *source,
00808                                                  const std::vector<std::string> &subspaces);
00811     }
00812 }
00813 
00814 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines