37 #include "ompl/geometric/HillClimbing.h"
55 double initialDistance;
57 bool wasValid = valid(state);
58 bool wasValidStart = wasValid;
60 bool wasSatisfied = goal.
isSatisfied(state, &initialDistance);
61 bool wasSatisfiedStart = wasSatisfied;
63 double bestDist = initialDistance;
67 unsigned int noUpdateSteps = 0;
72 ss->sampleUniformNear(test, state, nearDistance);
73 bool isValid = valid(test);
74 bool isSatisfied = goal.
isSatisfied(test, &tempDistance);
75 if (!wasValid && isValid)
77 si_->copyState(state, test);
79 wasSatisfied = isSatisfied;
83 if (wasValid == isValid)
85 if (!wasSatisfied && isSatisfied)
87 si_->copyState(state, test);
92 if (wasSatisfied == isSatisfied)
94 if (tempDistance < bestDist)
96 si_->copyState(state, test);
97 bestDist = tempDistance;
107 si_->freeState(test);
109 if (betterGoalDistance)
110 *betterGoalDistance = bestDist;
111 return (bestDist < initialDistance) || (!wasSatisfiedStart && wasSatisfied) || (!wasValidStart && wasValid);
bool tryToImprove(const base::GoalRegion &goal, base::State *state, double nearDistance, double *betterGoalDistance=NULL) const
Try to improve a state (reduce distance to goal). The updates are performed by sampling near the stat...
A boost shared pointer wrapper for ompl::base::StateSampler.
static const unsigned int MAX_CLIMB_NO_UPDATE_STEPS
Maximum number of consecutive failures to allow before giving up on improving a state. A failure consists of being unable to sample a state that is closer to the specified goal region.
Definition of an abstract state.
Definition of a goal region.
virtual bool isSatisfied(const State *st) const
Equivalent to calling isSatisfied(const State *, double *) with a NULL second argument.