All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
StateManifold.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator