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
00109 const std::string& getName(void) const;
00110
00112 void setName(const std::string &name);
00113
00117 int getType(void) const
00118 {
00119 return type_;
00120 }
00121
00123 bool includes(const StateSpacePtr &other) const;
00124
00127 bool covers(const StateSpacePtr &other) const;
00128
00135 virtual unsigned int getDimension(void) const = 0;
00136
00138 virtual double getMaximumExtent(void) const = 0;
00139
00141 virtual void enforceBounds(State *state) const = 0;
00142
00144 virtual bool satisfiesBounds(const State *state) const = 0;
00145
00147 virtual void copyState(State *destination, const State *source) const = 0;
00148
00150 virtual double distance(const State *state1, const State *state2) const = 0;
00151
00163 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00164
00170 virtual double getLongestValidSegmentFraction(void) const;
00171
00182 virtual void setLongestValidSegmentFraction(double segmentFraction);
00183
00185 virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00186
00193 void setValidSegmentCountFactor(unsigned int factor);
00194
00196 unsigned int getValidSegmentCountFactor(void) const;
00197
00199 virtual bool equalStates(const State *state1, const State *state2) const = 0;
00200
00204 virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
00205
00207 virtual StateSamplerPtr allocStateSampler(void) const = 0;
00208
00210 virtual State* allocState(void) const = 0;
00211
00213 virtual void freeState(State *state) const = 0;
00214
00222 void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
00223
00225 void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
00226
00229 virtual void registerProjections(void);
00230
00232 ProjectionEvaluatorPtr getProjection(const std::string &name) const;
00233
00235 ProjectionEvaluatorPtr getDefaultProjection(void) const;
00236
00238 bool hasProjection(const std::string &name) const;
00239
00241 bool hasDefaultProjection(void) const;
00242
00244 const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
00245
00249 virtual void printState(const State *state, std::ostream &out) const;
00250
00252 virtual void printSettings(std::ostream &out) const;
00253
00255 virtual void printProjections(std::ostream &out) const;
00256
00262 virtual void setup(void);
00263
00265 static void diagram(std::ostream &out);
00266
00267 protected:
00268
00270 static const std::string DEFAULT_PROJECTION_NAME;
00271
00273 int type_;
00274
00276 double maxExtent_;
00277
00279 double longestValidSegmentFraction_;
00280
00282 double longestValidSegment_;
00283
00285 unsigned int longestValidSegmentCountFactor_;
00286
00288 msg::Interface msg_;
00289
00291 std::map<std::string, ProjectionEvaluatorPtr> projections_;
00292
00293 private:
00294
00296 std::string name_;
00297 };
00298
00300 class CompoundStateSpace : public StateSpace
00301 {
00302 public:
00303
00305 typedef CompoundState StateType;
00306
00308 CompoundStateSpace(void);
00309
00311 CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
00312
00313 virtual ~CompoundStateSpace(void)
00314 {
00315 }
00316
00318 template<class T>
00319 T* as(const unsigned int index) const
00320 {
00322 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00323
00324 return static_cast<T*>(getSubSpace(index).get());
00325 }
00326
00328 template<class T>
00329 T* as(const std::string &name) const
00330 {
00332 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00333
00334 return static_cast<T*>(getSubSpace(name).get());
00335 }
00336
00337 virtual bool isCompound(void) const;
00338
00344 virtual void addSubSpace(const StateSpacePtr &component, double weight);
00345
00347 unsigned int getSubSpaceCount(void) const;
00348
00350 const StateSpacePtr& getSubSpace(const unsigned int index) const;
00351
00353 const StateSpacePtr& getSubSpace(const std::string& name) const;
00354
00356 unsigned int getSubSpaceIndex(const std::string& name) const;
00357
00359 bool hasSubSpace(const std::string &name) const;
00360
00362 double getSubSpaceWeight(const unsigned int index) const;
00363
00365 double getSubSpaceWeight(const std::string &name) const;
00366
00368 void setSubSpaceWeight(const unsigned int index, double weight);
00369
00371 void setSubSpaceWeight(const std::string &name, double weight);
00372
00374 const std::vector<StateSpacePtr>& getSubSpaces(void) const;
00375
00377 const std::vector<double>& getSubSpaceWeights(void) const;
00378
00382 bool isLocked(void) const;
00383
00389 virtual unsigned int getDimension(void) const;
00390
00391 virtual double getMaximumExtent(void) const;
00392
00393 virtual void enforceBounds(State *state) const;
00394
00395 virtual bool satisfiesBounds(const State *state) const;
00396
00397 virtual void copyState(State *destination, const State *source) const;
00398
00399 virtual double distance(const State *state1, const State *state2) const;
00400
00406 virtual void setLongestValidSegmentFraction(double segmentFraction);
00407
00410 virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00411
00412 virtual bool equalStates(const State *state1, const State *state2) const;
00413
00414 virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00415
00416 virtual StateSamplerPtr allocStateSampler(void) const;
00417
00418 virtual State* allocState(void) const;
00419
00420 virtual void freeState(State *state) const;
00421
00422 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00423
00426 virtual void printState(const State *state, std::ostream &out) const;
00427
00428 virtual void printSettings(std::ostream &out) const;
00429
00430 virtual void setup(void);
00431
00437 void lock(void);
00438
00439 protected:
00440
00442 void allocStateComponents(CompoundState *state) const;
00443
00445 std::vector<StateSpacePtr> components_;
00446
00448 unsigned int componentCount_;
00449
00451 std::vector<double> weights_;
00452
00454 bool locked_;
00455
00456 };
00457
00470 StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b);
00471
00478 StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b);
00479
00482 StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
00483
00486 StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b);
00497 int copyStateData(const StateSpacePtr &destS, State *dest,
00498 const StateSpacePtr &sourceS, const State *source);
00499 }
00500 }
00501
00502 #endif