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 #include "ompl/geometric/ik/HCIK.h"
00038
00039 namespace ompl
00040 {
00041 namespace magic
00042 {
00043
00048 static const unsigned int MAX_HCIK_NO_UPDATE_STEPS = 10;
00049 }
00050 }
00051
00052 bool ompl::geometric::HCIK::tryToImprove(const base::GoalRegion &goal, base::State *state, double nearDistance, double *betterGoalDistance) const
00053 {
00054 double tempDistance;
00055 double initialDistance;
00056
00057 bool wasValid = valid(state);
00058 bool wasValidStart = wasValid;
00059
00060 bool wasSatisfied = goal.isSatisfied(state, &initialDistance);
00061 bool wasSatisfiedStart = wasSatisfied;
00062
00063 double bestDist = initialDistance;
00064
00065 base::StateSamplerPtr ss = si_->allocStateSampler();
00066 base::State *test = si_->allocState();
00067 unsigned int noUpdateSteps = 0;
00068
00069 for (unsigned int i = 0 ; noUpdateSteps < magic::MAX_HCIK_NO_UPDATE_STEPS && i < maxImproveSteps_ ; ++i)
00070 {
00071 bool update = false;
00072 ss->sampleUniformNear(test, state, nearDistance);
00073 bool isValid = valid(test);
00074 bool isSatisfied = goal.isSatisfied(test, &tempDistance);
00075 if (!wasValid && isValid)
00076 {
00077 si_->copyState(state, test);
00078 wasValid = true;
00079 wasSatisfied = isSatisfied;
00080 update = true;
00081 }
00082 else
00083 if (wasValid == isValid)
00084 {
00085 if (!wasSatisfied && isSatisfied)
00086 {
00087 si_->copyState(state, test);
00088 wasSatisfied = true;
00089 update = true;
00090 }
00091 else
00092 if (wasSatisfied == isSatisfied)
00093 {
00094 if (tempDistance < bestDist)
00095 {
00096 si_->copyState(state, test);
00097 bestDist = tempDistance;
00098 update = true;
00099 }
00100 }
00101 }
00102 if (update)
00103 noUpdateSteps = 0;
00104 else
00105 noUpdateSteps++;
00106 }
00107 si_->freeState(test);
00108
00109 if (betterGoalDistance)
00110 *betterGoalDistance = bestDist;
00111 return (bestDist < initialDistance) || (!wasSatisfiedStart && wasSatisfied) || (!wasValidStart && wasValid);
00112 }