00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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