00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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);
00069 ~GAIK(void);
00070
00072 bool solve(double solveTime, const base::GoalRegion &goal, base::State *result,
00073 const std::vector<base::State*> &hint = std::vector<base::State*>());
00074
00076 void setMaxImproveSteps(unsigned int maxSteps)
00077 {
00078 hcik_.setMaxImproveSteps(maxSteps);
00079 }
00080
00082 unsigned int getMaxImproveSteps(void) const
00083 {
00084 return hcik_.getMaxImproveSteps();
00085 }
00086
00088 void setValidityCheck(bool valid)
00089 {
00090 checkValidity_ = valid;
00091 hcik_.setValidityCheck(valid);
00092 }
00093
00095 bool getValidityCheck(void) const
00096 {
00097 return checkValidity_;
00098 }
00099
00101 void setTryImprove(bool flag)
00102 {
00103 tryImprove_ = flag;
00104 }
00105
00107 bool getTryImprove(void) const
00108 {
00109 return tryImprove_;
00110 }
00111
00113 void setPoolSize(unsigned int size)
00114 {
00115 poolSize_ = size;
00116 }
00117
00119 unsigned int getPoolSize(void) const
00120 {
00121 return poolSize_;
00122 }
00123
00125 void setPoolMutationSize(unsigned int size)
00126 {
00127 poolMutation_ = size;
00128 }
00129
00131 unsigned int getPoolMutationSize(void) const
00132 {
00133 return poolMutation_;
00134 }
00135
00137 void setPoolRandomSize(unsigned int size)
00138 {
00139 poolRandom_ = size;
00140 }
00141
00143 unsigned int getPoolRandomSize(void) const
00144 {
00145 return poolRandom_;
00146 }
00147
00149 void setRange(double distance)
00150 {
00151 maxDistance_ = distance;
00152 }
00153
00155 double getRange(void) const
00156 {
00157 return maxDistance_;
00158 }
00159
00161 void clear(void);
00162
00163 private:
00164
00166 void tryToImprove(const base::GoalRegion &goal, base::State *state, double distance);
00167
00169 bool valid(const base::State *state) const
00170 {
00171 return checkValidity_ ? si_->isValid(state) : true;
00172 }
00173
00174 struct Individual
00175 {
00176 base::State *state;
00177 double distance;
00178 bool valid;
00179 };
00180
00181 struct IndividualSort
00182 {
00183 bool operator()(const Individual& a, const Individual& b)
00184 {
00185 if (a.valid == b.valid)
00186 return a.distance < b.distance;
00187 return a.valid;
00188 }
00189 };
00190
00191 HCIK hcik_;
00192 base::SpaceInformationPtr si_;
00193 base::StateSamplerPtr sampler_;
00194 std::vector<Individual> pool_;
00195 unsigned int poolSize_;
00196 unsigned int poolMutation_;
00197 unsigned int poolRandom_;
00198 unsigned int generations_;
00199 bool checkValidity_;
00200 bool tryImprove_;
00201
00202 double maxDistance_;
00203
00204 msg::Interface msg_;
00205 };
00206
00207 }
00208 }
00209
00210 #endif