All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
ControlManifold.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_CONTROL_CONTROL_MANIFOLD_
00038 #define OMPL_CONTROL_CONTROL_MANIFOLD_
00039 
00040 #include "ompl/base/StateManifold.h"
00041 #include "ompl/control/Control.h"
00042 #include "ompl/control/ControlSampler.h"
00043 #include "ompl/util/Console.h"
00044 #include "ompl/util/ClassForward.h"
00045 #include <boost/concept_check.hpp>
00046 #include <boost/noncopyable.hpp>
00047 #include <boost/function.hpp>
00048 #include <iostream>
00049 #include <vector>
00050 
00051 namespace ompl
00052 {
00053 
00054     namespace control
00055     {
00056 
00058         ClassForward(ControlManifold);
00059 
00064         typedef boost::function4<void, const base::State*, const Control*, const double, base::State*> StatePropagationFn;
00065 
00067         class ControlManifold : private boost::noncopyable
00068         {
00069         public:
00070 
00072             ControlManifold(const base::StateManifoldPtr &stateManifold);
00073 
00074             virtual ~ControlManifold(void);
00075 
00077             template<class T>
00078             T* as(void)
00079             {
00081                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, ControlManifold*>));
00082 
00083                 return static_cast<T*>(this);
00084             }
00085 
00087             template<class T>
00088             const T* as(void) const
00089             {
00091                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, ControlManifold*>));
00092 
00093                 return static_cast<const T*>(this);
00094             }
00095 
00097             const std::string& getName(void) const;
00098 
00100             void setName(const std::string &name);
00101 
00103             const base::StateManifoldPtr& getStateManifold(void) const
00104             {
00105                 return stateManifold_;
00106             }
00107 
00109             virtual unsigned int getDimension(void) const = 0;
00110 
00112             virtual Control* allocControl(void) const = 0;
00113 
00115             virtual void freeControl(Control *control) const = 0;
00116 
00118             virtual void copyControl(Control *destination, const Control *source) const = 0;
00119 
00121             virtual bool equalControls(const Control *control1, const Control *control2) const = 0;
00122 
00124             virtual void nullControl(Control *control) const = 0;
00125 
00127             virtual ControlSamplerPtr allocControlSampler(void) const = 0;
00128 
00144             virtual void propagate(const base::State *state, const Control* control, const double duration, base::State *result) const;
00145 
00150             virtual bool canPropagateBackward(void) const;
00151 
00153             void setPropagationFunction(const StatePropagationFn &fn);
00154 
00159             virtual double* getValueAddressAtIndex(Control *control, const unsigned int index) const;
00160 
00162             virtual void printControl(const Control *control, std::ostream &out) const;
00163 
00165             virtual void printSettings(std::ostream &out) const;
00166 
00168             virtual void setup(void);
00169 
00170         protected:
00171 
00173             base::StateManifoldPtr stateManifold_;
00174 
00176             StatePropagationFn     statePropagation_;
00177 
00178         private:
00179 
00181             std::string            name_;
00182         };
00183 
00185         class CompoundControlManifold : public ControlManifold
00186         {
00187         public:
00188 
00190             typedef CompoundControl ControlType;
00191 
00193             CompoundControlManifold(const base::StateManifoldPtr &stateManifold) : ControlManifold(stateManifold), componentCount_(0), locked_(false)
00194             {
00195             }
00196 
00197             virtual ~CompoundControlManifold(void)
00198             {
00199             }
00200 
00202             template<class T>
00203             T* as(const unsigned int index) const
00204             {
00206                 BOOST_CONCEPT_ASSERT((boost::Convertible<T*, ControlManifold*>));
00207 
00208                 return static_cast<T*>(getSubManifold(index).get());
00209             }
00210 
00212             virtual void addSubManifold(const ControlManifoldPtr &component);
00213 
00215             unsigned int getSubManifoldCount(void) const;
00216 
00218             const ControlManifoldPtr& getSubManifold(const unsigned int index) const;
00219 
00221             const ControlManifoldPtr& getSubManifold(const std::string &name) const;
00222 
00223             virtual unsigned int getDimension(void) const;
00224 
00225             virtual Control* allocControl(void) const;
00226 
00227             virtual void freeControl(Control *control) const;
00228 
00229             virtual void copyControl(Control *destination, const Control *source) const;
00230 
00231             virtual bool equalControls(const Control *control1, const Control *control2) const;
00232 
00233             virtual void nullControl(Control *control) const;
00234 
00235             virtual ControlSamplerPtr allocControlSampler(void) const;
00236 
00237             virtual void propagate(const base::State *state, const Control* control, const double duration, base::State *result) const;
00238 
00239             virtual bool canPropagateBackward(void) const;
00240 
00241             virtual double* getValueAddressAtIndex(Control *control, const unsigned int index) const;
00242 
00243             virtual void printControl(const Control *control, std::ostream &out = std::cout) const;
00244 
00245             virtual void printSettings(std::ostream &out) const;
00246 
00247             virtual void setup(void);
00248 
00254             void lock(void);
00255 
00256         protected:
00257 
00259             std::vector<ControlManifoldPtr> components_;
00260 
00262             unsigned int                    componentCount_;
00263 
00265             bool                            locked_;
00266         };
00267     }
00268 }
00269 
00270 #endif
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator