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