All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
StateSpace.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_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