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