All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
GAIK.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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_GEOMETRIC_IK_GAIK_
00038 #define OMPL_GEOMETRIC_IK_GAIK_
00039 
00040 #include "ompl/base/SpaceInformation.h"
00041 #include "ompl/base/GoalRegion.h"
00042 #include "ompl/geometric/ik/HCIK.h"
00043 #include "ompl/util/Console.h"
00044 
00045 namespace ompl
00046 {
00047 
00048     namespace geometric
00049     {
00050 
00063         class GAIK
00064         {
00065         public:
00066 
00068             GAIK(const base::SpaceInformationPtr &si) : hcik_(si), si_(si), poolSize_(100), poolMutation_(20), poolRandom_(30),
00069                                                         maxDistance_(0.0), msg_("GAIK")
00070             {
00071                 hcik_.setMaxImproveSteps(3);
00072                 setValidityCheck(true);
00073             }
00074 
00075             ~GAIK(void)
00076             {
00077             }
00078 
00080             bool solve(double solveTime, const base::GoalRegion &goal, base::State *result,
00081                        const std::vector<base::State*> &hint = std::vector<base::State*>());
00082 
00084             void setMaxImproveSteps(unsigned int maxSteps)
00085             {
00086                 hcik_.setMaxImproveSteps(maxSteps);
00087             }
00088 
00090             unsigned int getMaxImproveSteps(void) const
00091             {
00092                 return hcik_.getMaxImproveSteps();
00093             }
00094 
00096             void setValidityCheck(bool valid)
00097             {
00098                 checkValidity_ = valid;
00099                 hcik_.setValidityCheck(valid);
00100             }
00101 
00103             bool getValidityCheck(void) const
00104             {
00105                 return checkValidity_;
00106             }
00107 
00109             void setPoolSize(unsigned int size)
00110             {
00111                 poolSize_ = size;
00112             }
00113 
00115             unsigned int getPoolSize(void) const
00116             {
00117                 return poolSize_;
00118             }
00119 
00121             void setPoolMutationSize(unsigned int size)
00122             {
00123                 poolMutation_ = size;
00124             }
00125 
00127             unsigned int getPoolMutationSize(void) const
00128             {
00129                 return poolMutation_;
00130             }
00131 
00133             void setPoolRandomSize(unsigned int size)
00134             {
00135                 poolRandom_ = size;
00136             }
00137 
00139             unsigned int getPoolRandomSize(void) const
00140             {
00141                 return poolRandom_;
00142             }
00143 
00145             void setRange(double distance)
00146             {
00147                 maxDistance_ = distance;
00148             }
00149 
00151             double getRange(void) const
00152             {
00153                 return maxDistance_;
00154             }
00155 
00156         private:
00157 
00159             bool tryToImprove(const base::GoalRegion &goal, base::State *state, double distance);
00160 
00162             bool valid(const base::State *state) const
00163             {
00164                 return checkValidity_ ? si_->isValid(state) : true;
00165             }
00166 
00167 
00168             struct Individual
00169             {
00170                 base::State *state;
00171                 double       distance;
00172                 bool         valid;
00173             };
00174 
00175             struct IndividualSort
00176             {
00177                 bool operator()(const Individual& a, const Individual& b)
00178                 {
00179                     if (a.valid == b.valid)
00180                         return a.distance < b.distance;
00181                     return a.valid;
00182                 }
00183             };
00184 
00185             HCIK                                         hcik_;
00186             base::SpaceInformationPtr                    si_;
00187             unsigned int                                 poolSize_;
00188             unsigned int                                 poolMutation_;
00189             unsigned int                                 poolRandom_;
00190             bool                                         checkValidity_;
00191 
00192             double                                       maxDistance_;
00193 
00194             msg::Interface                               msg_;
00195         };
00196 
00197     }
00198 }
00199 
00200 #endif