Loading...
Searching...
No Matches
HillClimbing.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2008, Willow Garage, Inc.
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Willow Garage nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#include "ompl/geometric/HillClimbing.h"
38
39namespace ompl
40{
41 namespace magic
42 {
47 static const unsigned int MAX_CLIMB_NO_UPDATE_STEPS = 10;
48 }
49}
50
51bool ompl::geometric::HillClimbing::tryToImprove(const base::GoalRegion &goal, base::State *state, double nearDistance,
52 double *betterGoalDistance) const
53{
54 double tempDistance;
55 double initialDistance;
56
57 bool wasValid = valid(state);
58 bool wasValidStart = wasValid;
59
60 bool wasSatisfied = goal.isSatisfied(state, &initialDistance);
61 bool wasSatisfiedStart = wasSatisfied;
62
63 double bestDist = initialDistance;
64
65 base::StateSamplerPtr ss = si_->allocStateSampler();
66 base::State *test = si_->allocState();
67 unsigned int noUpdateSteps = 0;
68
69 for (unsigned int i = 0; noUpdateSteps < magic::MAX_CLIMB_NO_UPDATE_STEPS && i < maxImproveSteps_; ++i)
70 {
71 bool update = false;
72 ss->sampleUniformNear(test, state, nearDistance);
73 bool isValid = valid(test);
74 bool isSatisfied = goal.isSatisfied(test, &tempDistance);
75 if (!wasValid && isValid)
76 {
77 si_->copyState(state, test);
78 wasValid = true;
79 wasSatisfied = isSatisfied;
80 update = true;
81 }
82 else if (wasValid == isValid)
83 {
84 if (!wasSatisfied && isSatisfied)
85 {
86 si_->copyState(state, test);
87 wasSatisfied = true;
88 update = true;
89 }
90 else if (wasSatisfied == isSatisfied)
91 {
92 if (tempDistance < bestDist)
93 {
94 si_->copyState(state, test);
95 bestDist = tempDistance;
96 update = true;
97 }
98 }
99 }
100 if (update)
101 noUpdateSteps = 0;
102 else
103 noUpdateSteps++;
104 }
105 si_->freeState(test);
106
107 if (betterGoalDistance)
108 *betterGoalDistance = bestDist;
109 return (bestDist < initialDistance) || (!wasSatisfiedStart && wasSatisfied) || (!wasValidStart && wasValid);
110}
Definition of a goal region.
Definition: GoalRegion.h:48
bool isSatisfied(const State *st) const override
Equivalent to calling isSatisfied(const State *, double *) with a nullptr second argument.
Definition: GoalRegion.cpp:47
Definition of an abstract state.
Definition: State.h:50
bool tryToImprove(const base::GoalRegion &goal, base::State *state, double nearDistance, double *betterGoalDistance=nullptr) const
Try to improve a state (reduce distance to goal). The updates are performed by sampling near the stat...
static const unsigned int MAX_CLIMB_NO_UPDATE_STEPS
Maximum number of consecutive failures to allow before giving up on improving a state....
Main namespace. Contains everything in this library.