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 virtual bool isHybrid(void) const;
00118
00120 const std::string& getName(void) const;
00121
00123 void setName(const std::string &name);
00124
00128 int getType(void) const
00129 {
00130 return type_;
00131 }
00132
00134 bool includes(const StateSpacePtr &other) const;
00135
00138 bool covers(const StateSpacePtr &other) const;
00139
00146 virtual unsigned int getDimension(void) const = 0;
00147
00154 virtual double getMaximumExtent(void) const = 0;
00155
00158 virtual void enforceBounds(State *state) const = 0;
00159
00162 virtual bool satisfiesBounds(const State *state) const = 0;
00163
00165 virtual void copyState(State *destination, const State *source) const = 0;
00166
00169 virtual double distance(const State *state1, const State *state2) const = 0;
00170
00182 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00183
00189 virtual double getLongestValidSegmentFraction(void) const;
00190
00201 virtual void setLongestValidSegmentFraction(double segmentFraction);
00202
00204 virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00205
00212 void setValidSegmentCountFactor(unsigned int factor);
00213
00215 unsigned int getValidSegmentCountFactor(void) const;
00216
00218 virtual bool equalStates(const State *state1, const State *state2) const = 0;
00219
00223 virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
00224
00226 virtual StateSamplerPtr allocStateSampler(void) const = 0;
00227
00229 virtual State* allocState(void) const = 0;
00230
00232 virtual void freeState(State *state) const = 0;
00233
00241 void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
00242
00244 void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
00245
00248 virtual void registerProjections(void);
00249
00251 ProjectionEvaluatorPtr getProjection(const std::string &name) const;
00252
00254 ProjectionEvaluatorPtr getDefaultProjection(void) const;
00255
00257 bool hasProjection(const std::string &name) const;
00258
00260 bool hasDefaultProjection(void) const;
00261
00263 const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
00264
00271 virtual void printState(const State *state, std::ostream &out) const;
00272
00274 virtual void printSettings(std::ostream &out) const;
00275
00277 virtual void printProjections(std::ostream &out) const;
00278
00281 virtual void sanityChecks(void) const;
00282
00284 static void Diagram(std::ostream &out);
00285
00293 virtual void setup(void);
00294
00295 protected:
00296
00298 static const std::string DEFAULT_PROJECTION_NAME;
00299
00301 int type_;
00302
00304 double maxExtent_;
00305
00307 double longestValidSegmentFraction_;
00308
00310 double longestValidSegment_;
00311
00313 unsigned int longestValidSegmentCountFactor_;
00314
00316 msg::Interface msg_;
00317
00319 std::map<std::string, ProjectionEvaluatorPtr> projections_;
00320
00321 private:
00322
00324 std::string name_;
00325 };
00326
00328 class CompoundStateSpace : public StateSpace
00329 {
00330 public:
00331
00333 typedef CompoundState StateType;
00334
00336 CompoundStateSpace(void);
00337
00339 CompoundStateSpace(const std::vector<StateSpacePtr> &components, const std::vector<double> &weights);
00340
00341 virtual ~CompoundStateSpace(void)
00342 {
00343 }
00344
00346 template<class T>
00347 T* as(const unsigned int index) const
00348 {
00350 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00351
00352 return static_cast<T*>(getSubSpace(index).get());
00353 }
00354
00356 template<class T>
00357 T* as(const std::string &name) const
00358 {
00360 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateSpace*>));
00361
00362 return static_cast<T*>(getSubSpace(name).get());
00363 }
00364
00365 virtual bool isCompound(void) const;
00366
00367 virtual bool isHybrid(void) const;
00368
00374 virtual void addSubSpace(const StateSpacePtr &component, double weight);
00375
00377 unsigned int getSubSpaceCount(void) const;
00378
00380 const StateSpacePtr& getSubSpace(const unsigned int index) const;
00381
00383 const StateSpacePtr& getSubSpace(const std::string& name) const;
00384
00386 unsigned int getSubSpaceIndex(const std::string& name) const;
00387
00389 bool hasSubSpace(const std::string &name) const;
00390
00392 double getSubSpaceWeight(const unsigned int index) const;
00393
00395 double getSubSpaceWeight(const std::string &name) const;
00396
00398 void setSubSpaceWeight(const unsigned int index, double weight);
00399
00401 void setSubSpaceWeight(const std::string &name, double weight);
00402
00404 const std::vector<StateSpacePtr>& getSubSpaces(void) const;
00405
00407 const std::vector<double>& getSubSpaceWeights(void) const;
00408
00412 bool isLocked(void) const;
00413
00419 virtual unsigned int getDimension(void) const;
00420
00421 virtual double getMaximumExtent(void) const;
00422
00423 virtual void enforceBounds(State *state) const;
00424
00425 virtual bool satisfiesBounds(const State *state) const;
00426
00427 virtual void copyState(State *destination, const State *source) const;
00428
00429 virtual double distance(const State *state1, const State *state2) const;
00430
00436 virtual void setLongestValidSegmentFraction(double segmentFraction);
00437
00440 virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00441
00442 virtual bool equalStates(const State *state1, const State *state2) const;
00443
00444 virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00445
00446 virtual StateSamplerPtr allocStateSampler(void) const;
00447
00448 virtual State* allocState(void) const;
00449
00450 virtual void freeState(State *state) const;
00451
00452 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00453
00456 virtual void printState(const State *state, std::ostream &out) const;
00457
00458 virtual void printSettings(std::ostream &out) const;
00459
00460 virtual void setup(void);
00461
00467 void lock(void);
00468
00469 protected:
00470
00472 void allocStateComponents(CompoundState *state) const;
00473
00475 std::vector<StateSpacePtr> components_;
00476
00478 unsigned int componentCount_;
00479
00481 std::vector<double> weights_;
00482
00484 bool locked_;
00485
00486 };
00487
00500 StateSpacePtr operator+(const StateSpacePtr &a, const StateSpacePtr &b);
00501
00508 StateSpacePtr operator-(const StateSpacePtr &a, const StateSpacePtr &b);
00509
00512 StateSpacePtr operator-(const StateSpacePtr &a, const std::string &name);
00513
00516 StateSpacePtr operator*(const StateSpacePtr &a, const StateSpacePtr &b);
00527 int copyStateData(const StateSpacePtr &destS, State *dest,
00528 const StateSpacePtr &sourceS, const State *source);
00529 }
00530 }
00531
00532 #endif