ompl/extensions/morse/MorseEnvironment.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: Caleb Voss */
00036 
00037 #ifndef OMPL_EXTENSION_MORSE_ENVIRONMENT_
00038 #define OMPL_EXTENSION_MORSE_ENVIRONMENT_
00039 
00040 #include "ompl/config.h"
00041 #if OMPL_EXTENSION_MORSE == 0
00042 #  error MORSE extension not built
00043 #endif
00044 
00045 #include "ompl/base/State.h"
00046 #include "ompl/util/ClassForward.h"
00047 
00048 #include "boost/thread/mutex.hpp"
00049 
00050 #include <limits>
00051 #include <vector>
00052 
00053 namespace ompl
00054 {
00055     namespace base
00056     {
00057 
00059 
00060         OMPL_CLASS_FORWARD(MorseEnvironment);
00062 
00067         class MorseEnvironment
00068         {
00069         public:
00070             
00072             const unsigned int controlDim_;
00073             
00075             const std::vector<double> controlBounds_;
00076             
00078             const unsigned int rigidBodies_;
00079             
00081             std::vector<double> positionBounds_;
00082             
00084             std::vector<double> linvelBounds_;
00085             
00087             std::vector<double> angvelBounds_;
00088             
00090             double stepSize_;
00091 
00093             unsigned int minControlSteps_;
00094 
00096             unsigned int maxControlSteps_;
00097             
00099             bool simRunning_;
00100             
00102             mutable boost::mutex mutex_;
00103 
00104             MorseEnvironment(const unsigned int controlDim, const std::vector<double> &controlBounds,
00105                 const unsigned int rigidBodies, const std::vector<double> &positionBounds,
00106                 const std::vector<double> &linvelBounds, const std::vector<double> &angvelBounds,
00107                 const double stepSize, const unsigned int minControlSteps, const unsigned int maxControlSteps)
00108                  : controlDim_(controlDim), controlBounds_(controlBounds), rigidBodies_(rigidBodies),
00109                    positionBounds_(positionBounds), linvelBounds_(linvelBounds), angvelBounds_(angvelBounds),
00110                    stepSize_(stepSize), minControlSteps_(minControlSteps), maxControlSteps_(maxControlSteps),
00111                    simRunning_(true)
00112             {
00113                 // Replace infinite bounds with very large bounds, so, e.g., sampling can still work
00114                 for (unsigned int i = 0; i < positionBounds_.size(); i++)
00115                 {
00116                     if (positionBounds_[i]==std::numeric_limits<double>::infinity())
00117                         positionBounds_[i] = std::numeric_limits<double>::max()/2;
00118                     else if (positionBounds_[i]==-std::numeric_limits<double>::infinity())
00119                         positionBounds_[i] = -std::numeric_limits<double>::max()/2;
00120                 }
00121                 for (unsigned int i = 0; i < linvelBounds_.size(); i++)
00122                 {
00123                     if (linvelBounds_[i]==std::numeric_limits<double>::infinity())
00124                         linvelBounds_[i] = std::numeric_limits<double>::max()/2;
00125                     else if (linvelBounds_[i]==-std::numeric_limits<double>::infinity())
00126                         linvelBounds_[i] = -std::numeric_limits<double>::max()/2;
00127                 }
00128                 for (unsigned int i = 0; i < angvelBounds_.size(); i++)
00129                 {
00130                     if (angvelBounds_[i]==std::numeric_limits<double>::infinity())
00131                         angvelBounds_[i] = std::numeric_limits<double>::max()/2;
00132                     else if (angvelBounds_[i]==-std::numeric_limits<double>::infinity())
00133                         angvelBounds_[i] = -std::numeric_limits<double>::max()/2;
00134                 }
00135             }
00136 
00137             ~MorseEnvironment()
00138             {
00139             }
00140 
00142             void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const;
00143             
00144             
00145             // These functions require interprocess communication and are left to be implemented in Python
00146             
00148             virtual void readState(State *state) = 0;
00149             
00151             virtual void writeState(const State *state) = 0;
00152             
00154             virtual void applyControl(const std::vector<double> &control) = 0;
00155             
00157             virtual void worldStep(const double dur) = 0;
00158         };
00159     }
00160 }
00161 
00162 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines