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/util/Console.h"
00045 #include "ompl/util/ClassForward.h"
00046 #include <boost/concept_check.hpp>
00047 #include <boost/noncopyable.hpp>
00048 #include <iostream>
00049 #include <vector>
00050 #include <string>
00051 #include <map>
00052
00053 namespace ompl
00054 {
00055 namespace base
00056 {
00057
00059 ClassForward(StateSpace);
00060
00070 class StateSpace : private boost::noncopyable
00071 {
00072 public:
00073
00075 typedef State StateType;
00076
00078 StateSpace(void);
00079
00080 virtual ~StateSpace(void);
00081
00083 template<class T>
00084 T* as(void)
00085 {
00087 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00088
00089 return static_cast<T*>(this);
00090 }
00091
00093 template<class T>
00094 const T* as(void) const
00095 {
00097 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00098
00099 return static_cast<const T*>(this);
00100 }
00101
00106 virtual bool isCompound(void) const;
00107
00114 virtual bool isDiscrete(void) const;
00115
00117 const std::string& getName(void) const;
00118
00120 void setName(const std::string &name);
00121
00125 int getType(void) const
00126 {
00127 return type_;
00128 }
00129
00131 bool includes(const StateSpacePtr &other) const;
00132
00135 bool covers(const StateSpacePtr &other) const;
00136
00143 virtual unsigned int getDimension(void) const = 0;
00144
00151 virtual double getMaximumExtent(void) const = 0;
00152
00155 virtual void enforceBounds(State *state) const = 0;
00156
00159 virtual bool satisfiesBounds(const State *state) const = 0;
00160
00162 virtual void copyState(State *destination, const State *source) const = 0;
00163
00166 virtual double distance(const State *state1, const State *state2) const = 0;
00167
00179 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00180
00186 virtual double getLongestValidSegmentFraction(void) const;
00187
00198 virtual void setLongestValidSegmentFraction(double segmentFraction);
00199
00201 virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00202
00209 void setValidSegmentCountFactor(unsigned int factor);
00210
00212 unsigned int getValidSegmentCountFactor(void) const;
00213
00215 virtual bool equalStates(const State *state1, const State *state2) const = 0;
00216
00220 virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
00221
00223 virtual StateSamplerPtr allocStateSampler(void) const = 0;
00224
00226 virtual State* allocState(void) const = 0;
00227
00229 virtual void freeState(State *state) const = 0;
00230
00238 void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
00239
00241 void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
00242
00245 virtual void registerProjections(void);
00246
00248 ProjectionEvaluatorPtr getProjection(const std::string &name) const;
00249
00251 ProjectionEvaluatorPtr getDefaultProjection(void) const;
00252
00254 bool hasProjection(const std::string &name) const;
00255
00257 bool hasDefaultProjection(void) const;
00258
00260 const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
00261
00268 virtual void printState(const State *state, std::ostream &out) const;
00269
00271 virtual void printSettings(std::ostream &out) const;
00272
00274 virtual void printProjections(std::ostream &out) const;
00275
00278 virtual void sanityChecks(void) const;
00279
00281 static void Diagram(std::ostream &out);
00282
00290 virtual void setup(void);
00291
00292 protected:
00293
00295 static const std::string DEFAULT_PROJECTION_NAME;
00296
00298 int type_;
00299
00301 double maxExtent_;
00302
00304 double longestValidSegmentFraction_;
00305
00307 double longestValidSegment_;
00308
00310 unsigned int longestValidSegmentCountFactor_;
00311
00313 msg::Interface msg_;
00314
00316 std::map<std::string, ProjectionEvaluatorPtr> projections_;
00317
00318 private:
00319
00321 std::string name_;
00322 };
00323
00325 class CompoundStateSpace : public StateSpace
00326 {
00327 public:
00328
00330 typedef CompoundState StateType;
00331
00333 CompoundStateSpace(void);
00334
00336 CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
00337
00338 virtual ~CompoundStateSpace(void)
00339 {
00340 }
00341
00343 template<class T>
00344 T* as(const unsigned int index) const
00345 {
00347 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00348
00349 return static_cast<T*>(getSubSpace(index).get());
00350 }
00351
00353 template<class T>
00354 T* as(const std::string &name) const
00355 {
00357 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00358
00359 return static_cast<T*>(getSubSpace(name).get());
00360 }
00361
00362 virtual bool isCompound(void) const;
00363
00369 virtual void addSubSpace(const StateSpacePtr &component, double weight);
00370
00372 unsigned int getSubSpaceCount(void) const;
00373
00375 const StateSpacePtr& getSubSpace(const unsigned int index) const;
00376
00378 const StateSpacePtr& getSubSpace(const std::string& name) const;
00379
00381 unsigned int getSubSpaceIndex(const std::string& name) const;
00382
00384 bool hasSubSpace(const std::string &name) const;
00385
00387 double getSubSpaceWeight(const unsigned int index) const;
00388
00390 double getSubSpaceWeight(const std::string &name) const;
00391
00393 void setSubSpaceWeight(const unsigned int index, double weight);
00394
00396 void setSubSpaceWeight(const std::string &name, double weight);
00397
00399 const std::vector<StateSpacePtr>& getSubSpaces(void) const;
00400
00402 const std::vector<double>& getSubSpaceWeights(void) const;
00403
00407 bool isLocked(void) const;
00408
00414 virtual unsigned int getDimension(void) const;
00415
00416 virtual double getMaximumExtent(void) const;
00417
00418 virtual void enforceBounds(State *state) const;
00419
00420 virtual bool satisfiesBounds(const State *state) const;
00421
00422 virtual void copyState(State *destination, const State *source) const;
00423
00424 virtual double distance(const State *state1, const State *state2) const;
00425
00431 virtual void setLongestValidSegmentFraction(double segmentFraction);
00432
00435 virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00436
00437 virtual bool equalStates(const State *state1, const State *state2) const;
00438
00439 virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00440
00441 virtual StateSamplerPtr allocStateSampler(void) const;
00442
00443 virtual State* allocState(void) const;
00444
00445 virtual void freeState(State *state) const;
00446
00447 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00448
00451 virtual void printState(const State *state, std::ostream &out) const;
00452
00453 virtual void printSettings(std::ostream &out) const;
00454
00455 virtual void setup(void);
00456
00462 void lock(void);
00463
00464 protected:
00465
00467 void allocStateComponents(CompoundState *state) const;
00468
00470 std::vector<StateSpacePtr> components_;
00471
00473 unsigned int componentCount_;
00474
00476 std::vector<double> weights_;
00477
00479 bool locked_;
00480
00481 };
00482
00495 StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b);
00496
00503 StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b);
00504
00507 StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
00508
00511 StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b);
00522 int copyStateData(const StateSpacePtr &destS, State *dest,
00523 const StateSpacePtr &sourceS, const State *source);
00524 }
00525 }
00526
00527 #endif