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 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