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_MANIFOLD_
00038 #define OMPL_BASE_STATE_MANIFOLD_
00039
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/StateManifoldTypes.h"
00042 #include "ompl/base/ManifoldStateSampler.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(StateManifold);
00060
00070 class StateManifold : private boost::noncopyable
00071 {
00072 public:
00073
00075 typedef State StateType;
00076
00078 StateManifold(void);
00079
00080 virtual ~StateManifold(void);
00081
00083 template<class T>
00084 T* as(void)
00085 {
00087 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateManifold*>));
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*, StateManifold*>));
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 StateManifoldPtr &other) const;
00124
00129 bool covers(const StateManifoldPtr &other) const;
00130
00137 virtual unsigned int getDimension(void) const = 0;
00138
00140 virtual double getMaximumExtent(void) const = 0;
00141
00143 virtual void enforceBounds(State *state) const = 0;
00144
00146 virtual bool satisfiesBounds(const State *state) const = 0;
00147
00149 virtual void copyState(State *destination, const State *source) const = 0;
00150
00152 virtual double distance(const State *state1, const State *state2) const = 0;
00153
00158 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00159
00165 virtual double getLongestValidSegmentFraction(void) const;
00166
00177 virtual void setLongestValidSegmentFraction(double segmentFraction);
00178
00180 virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00181
00188 void setValidSegmentCountFactor(unsigned int factor);
00189
00191 unsigned int getValidSegmentCountFactor(void) const;
00192
00194 virtual bool equalStates(const State *state1, const State *state2) const = 0;
00195
00199 virtual void interpolate(const State *from, const State *to, const double t, State *state) const = 0;
00200
00202 virtual ManifoldStateSamplerPtr allocStateSampler(void) const = 0;
00203
00205 virtual State* allocState(void) const = 0;
00206
00208 virtual void freeState(State *state) const = 0;
00209
00217 void registerProjection(const std::string &name, const ProjectionEvaluatorPtr &projection);
00218
00220 void registerDefaultProjection(const ProjectionEvaluatorPtr &projection);
00221
00224 virtual void registerProjections(void);
00225
00227 ProjectionEvaluatorPtr getProjection(const std::string &name) const;
00228
00230 ProjectionEvaluatorPtr getDefaultProjection(void) const;
00231
00233 bool hasProjection(const std::string &name) const;
00234
00236 bool hasDefaultProjection(void) const;
00237
00239 const std::map<std::string, ProjectionEvaluatorPtr>& getRegisteredProjections(void) const;
00240
00244 virtual void printState(const State *state, std::ostream &out) const;
00245
00247 virtual void printSettings(std::ostream &out) const;
00248
00250 virtual void printProjections(std::ostream &out) const;
00251
00257 virtual void setup(void);
00258
00260 static void diagram(std::ostream &out);
00261
00262 protected:
00263
00265 static const std::string DEFAULT_PROJECTION_NAME;
00266
00268 int type_;
00269
00271 double maxExtent_;
00272
00274 double longestValidSegmentFraction_;
00275
00277 double longestValidSegment_;
00278
00280 unsigned int longestValidSegmentCountFactor_;
00281
00283 msg::Interface msg_;
00284
00286 std::map<std::string, ProjectionEvaluatorPtr> projections_;
00287
00288 private:
00289
00291 std::string name_;
00292 };
00293
00295 class CompoundStateManifold : public StateManifold
00296 {
00297 public:
00298
00300 typedef CompoundState StateType;
00301
00303 CompoundStateManifold(void);
00304
00306 CompoundStateManifold(const std::vector<StateManifoldPtr> &components, const std::vector<double> &weights);
00307
00308 virtual ~CompoundStateManifold(void)
00309 {
00310 }
00311
00313 template<class T>
00314 T* as(const unsigned int index) const
00315 {
00317 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateManifold*>));
00318
00319 return static_cast<T*>(getSubManifold(index).get());
00320 }
00321
00323 template<class T>
00324 T* as(const std::string &name) const
00325 {
00327 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, StateManifold*>));
00328
00329 return static_cast<T*>(getSubManifold(name).get());
00330 }
00331
00332 virtual bool isCompound(void) const;
00333
00339 virtual void addSubManifold(const StateManifoldPtr &component, double weight);
00340
00342 unsigned int getSubManifoldCount(void) const;
00343
00345 const StateManifoldPtr& getSubManifold(const unsigned int index) const;
00346
00348 const StateManifoldPtr& getSubManifold(const std::string& name) const;
00349
00351 unsigned int getSubManifoldIndex(const std::string& name) const;
00352
00354 bool hasSubManifold(const std::string &name) const;
00355
00357 double getSubManifoldWeight(const unsigned int index) const;
00358
00360 double getSubManifoldWeight(const std::string &name) const;
00361
00363 void setSubManifoldWeight(const unsigned int index, double weight);
00364
00366 void setSubManifoldWeight(const std::string &name, double weight);
00367
00369 const std::vector<StateManifoldPtr>& getSubManifolds(void) const;
00370
00372 const std::vector<double>& getSubManifoldWeights(void) const;
00373
00377 bool isLocked(void) const;
00378
00384 virtual unsigned int getDimension(void) const;
00385
00386 virtual double getMaximumExtent(void) const;
00387
00388 virtual void enforceBounds(State *state) const;
00389
00390 virtual bool satisfiesBounds(const State *state) const;
00391
00392 virtual void copyState(State *destination, const State *source) const;
00393
00394 virtual double distance(const State *state1, const State *state2) const;
00395
00401 virtual void setLongestValidSegmentFraction(double segmentFraction);
00402
00405 virtual unsigned int validSegmentCount(const State *state1, const State *state2) const;
00406
00407 virtual bool equalStates(const State *state1, const State *state2) const;
00408
00409 virtual void interpolate(const State *from, const State *to, const double t, State *state) const;
00410
00411 virtual ManifoldStateSamplerPtr allocStateSampler(void) const;
00412
00413 virtual State* allocState(void) const;
00414
00415 virtual void freeState(State *state) const;
00418 virtual double* getValueAddressAtIndex(State *state, const unsigned int index) const;
00419
00420 virtual void printState(const State *state, std::ostream &out) const;
00421
00422 virtual void printSettings(std::ostream &out) const;
00423
00424 virtual void setup(void);
00425
00431 void lock(void);
00432
00433 protected:
00434
00436 void allocStateComponents(CompoundState *state) const;
00437
00439 std::vector<StateManifoldPtr> components_;
00440
00442 unsigned int componentCount_;
00443
00445 std::vector<double> weights_;
00446
00448 bool locked_;
00449
00450 };
00451
00464 StateManifoldPtr operator+(const StateManifoldPtr &a, const StateManifoldPtr &b);
00465
00472 StateManifoldPtr operator-(const StateManifoldPtr &a, const StateManifoldPtr &b);
00473
00476 StateManifoldPtr operator-(const StateManifoldPtr &a, const std::string &name);
00477
00480 StateManifoldPtr operator*(const StateManifoldPtr &a, const StateManifoldPtr &b);
00491 int copyStateData(const StateManifoldPtr &destM, State *dest,
00492 const StateManifoldPtr &sourceM, const State *source);
00493 }
00494 }
00495
00496 #endif