HOG2
IncrementalBTS.h
Go to the documentation of this file.
1 //
2 // IncrementalBTS.h
3 // hog2
4 //
5 // This is an incremental version of Budgeted Tree Search - IBEX on trees with Bounded DFS at the low level.
6 // This code isn't meant for production - just to animate how the algorithm works as a comparison to IDA*.
7 // See (Helmert et al, IJCAI 2019) for details of the algorithm
8 //
9 
10 #ifndef IncrementalBTS_h
11 #define IncrementalBTS_h
12 
13 #include "Heuristic.h"
14 
15 enum IBEXStage {
16  kCompletedFullStage,
17  kIDAStarSearch,
18  kExponentialSearch,
19  kBinarySearch
20 };
21 
22 template <class state, class action>
23 class IncrementalBTS {
24 public:
25  IncrementalBTS(double initialBound = 0) :bound(initialBound), initialBound(initialBound), nextBound(initialBound)
26  { ResetNodeCount(); previousBound = 0; }
27  bool InitializeSearch(SearchEnvironment<state, action> *env, state from, state to, Heuristic<state> *h,
28  std::vector<state> &thePath);
29  void GetPath(SearchEnvironment<state, action> *env, state from, state to, Heuristic<state> *h,
30  std::vector<state> &thePath);
31  void GetPath(SearchEnvironment<state, action> *env, state from, state to,
32  std::vector<action> &thePath);
33  bool DoSingleSearchStep(std::vector<state> &thePath);
34  uint64_t GetNodesExpanded() { return nodesExpanded; }
35  uint64_t GetIterationNodesExpanded() { return data.nodesExpanded; }
36  uint64_t GetNodesTouched() { return nodesTouched; }
37  void ResetNodeCount()
38  {
39  nodesExpanded = nodesTouched = 0;
40  newNodeCount = newNodesLastIteration = 0;
41  }
42  void Reset() { bound = initialBound; nextBound = initialBound; solutionPath.clear();
43  history.clear(); search.clear(); ResetNodeCount(); previousBound = 0; }
44  void OpenGLDraw();
45  void Draw(Graphics::Display &display) const;
46  state GetCurrentState() const
47  {
48  if (search.size() > 0)
49  return search.back().currState;
50  if (flesseq(solutionCost, data.solutionInterval.lowerBound))
51  return goal;
52  return start;
53 
54  }
55  void GetCurrentPath(std::vector<state> &p) const
56  { p.clear(); for (auto &i : search) p.push_back(i.currState); }
57  double GetCurrentFLimit() { return std::min(bound, solutionCost); }
58  double GetNextFLimit() { return nextBound; }
59  uint64_t GetNewNodesLastIteration() { return newNodesLastIteration; }
60  const uint64_t c1 = 2;
61  const uint64_t c2 = 8;
62  const uint64_t gamma = 2;
63  const int infiniteWorkBound = -1;
64  void GetGlobalCostInterval(double &lower, double &upper)
65  { lower = data.solutionInterval.lowerBound; upper = data.solutionInterval.upperBound; }
66  void GetNodeInterval(uint64_t &lower, uint64_t &upper)
67  { lower = data.nodeLB*c1; upper = data.workBound; }
68  std::string stage;
69  std::string fEquation;
70  IBEXStage currStage;
71 private:
72  struct costInterval {
73  double lowerBound;
74  double upperBound;
75  costInterval &operator&=(const costInterval &i)
76  {
77  lowerBound = std::max(lowerBound, i.lowerBound);
78  upperBound = std::min(upperBound, i.upperBound);
79  return *this;
80  }
81  };
82  struct IBEXData {
83  uint64_t nodesExpanded;
84  uint64_t workBound;
85  uint64_t nodeLB;
86  costInterval solutionInterval;
87  int delta;
88  };
89  IBEXData data;
90  struct searchData {
91  double f_below;
92  double f_above;
93  };
94  searchData sd;
95  enum kSearchStatus {
96  kGoingDown,
97  kGoingAcross
98  };
99  struct currSearchState {
100  state currState;
101  kSearchStatus status;
102  double pathCost;
103  std::vector<state> succ;
104  };
105  std::vector<currSearchState> search;
106  double solutionCost;
107  bool IterationComplete() { return search.size() == 0; }
108  unsigned long nodesExpanded, nodesTouched;
109 
110  void SetupIteration(double cost);
111  bool StepIteration();
112 
113  std::vector<std::pair<state, double>> history;
114  std::vector<state> solutionPath;
115  state start, goal;
116  double previousBound;
117  double bound;
118  double initialBound;
119  double nextBound;
120  SearchEnvironment<state, action> *env;
121  Heuristic<state> *h;
122  std::vector<state> succ;
123  uint64_t newNodeCount, newNodesLastIteration;
124 };
125 
126 template <class state, class action>
127 void IncrementalBTS<state, action>::GetPath(SearchEnvironment<state, action> *e, state from, state to,
128  Heuristic<state> *h, std::vector<state> &thePath)
129 {
130  if (InitializeSearch(e, from, to, h, thePath))
131  return;
132  while (!DoSingleSearchStep(thePath))
133  {}
134 }
135 
136 template <class state, class action>
137 void IncrementalBTS<state, action>::GetPath(SearchEnvironment<state, action> *e, state from, state to,
138  std::vector<action> &thePath)
139 {
140 
141 }
142 
143 template <class state, class action>
144 bool IncrementalBTS<state, action>::InitializeSearch(SearchEnvironment<state, action> *e, state from, state to, Heuristic<state> *h,
145  std::vector<state> &thePath)
146 {
147  Reset();
148  if (from==to)
149  {
150  thePath.clear();
151  thePath.push_back(from);
152  return true;
153  }
154  start = from;
155  goal = to;
156  this->env = e;
157  start = from;
158  this->h = h;
159 
160  solutionPath.clear();
161 
162  // Setup BTS control data
163  data.nodesExpanded = 0;
164  data.workBound = infiniteWorkBound;
165  data.nodeLB = 0;
166  data.solutionInterval.lowerBound = h->HCost(start, goal);
167  data.solutionInterval.upperBound = DBL_MAX;
168  data.delta = 0;
169  fEquation = to_string_with_precision(data.solutionInterval.lowerBound, 2);
170 
171  solutionCost = DBL_MAX;
172  SetupIteration(h->HCost(start, goal));
173  stage = "IDA* ITERATION";
174  currStage = kIDAStarSearch;
175  return false;
176 }
177 
178 template <class state, class action>
179 void IncrementalBTS<state, action>::SetupIteration(double cost)
180 {
181  search.resize(1);
182  search.back().currState = start;
183  search.back().status = kGoingDown;
184  search.back().pathCost = 0;
185  previousBound = bound;
186  bound = cost;//nextBound;
187  nextBound = -1;
188  printf("Starting iteration bound %1.1f\n", bound);
189  newNodesLastIteration = newNodeCount;
190  newNodeCount = 0;
191  data.nodesExpanded = 0;
192  sd.f_below = 0;
193  sd.f_above = DBL_MAX;
194 }
195 
196 template <class state, class action>
197 bool IncrementalBTS<state, action>::StepIteration()
198 {
199  if (env->GoalTest(search.back().currState, goal))
200  {
201  if (fless(search.back().pathCost, solutionCost))
202  {
203  solutionPath.clear();
204  for (size_t x = 0; x < search.size(); x++)
205  solutionPath.push_back(search[x].currState);
206  solutionCost = search.back().pathCost;
207  printf("Found solution cost %f!", solutionCost);
208  }
209  if (!flesseq(solutionCost, data.solutionInterval.lowerBound))
210  search.pop_back();
211  return false;
212  }
213 
214  if (search.back().status == kGoingDown)
215  {
216  double f = search.back().pathCost+h->HCost(search.back().currState, goal);
217  // exceeded path cost bound
218  if (fgreater(f, bound))
219  {
220  //printf("Above bound: %f/%f\n", bound, f);
221  sd.f_above = std::min(sd.f_above, f);
222  if (nextBound == -1)
223  nextBound = f;
224  else if (fless(f, nextBound))
225  nextBound = f;
226 
227  search.pop_back();
228  return false;
229  }
230  if (fgreater(f, solutionCost))
231  {
232  search.pop_back();
233  return false;
234  }
235  if (flesseq(f, bound))
236  sd.f_below = std::max(sd.f_below, f);
237 // data.solutionInterval.upperBound = std::max(data.solutionInterval.upperBound, f);
238 
239  if (fgreater(f, previousBound) && flesseq(f, bound))
240  newNodeCount++;
241 
242  // continue search
243  //printf("Generating next set of successors\n");
244  search.back().status = kGoingAcross;
245  env->GetSuccessors(search.back().currState, search.back().succ);
246  nodesExpanded++;
247  for (size_t x = 0; x < search.back().succ.size(); x++)
248  {
249  if (search.size() > 1 && search.back().succ[x] == search[search.size()-2].currState)
250  {
251  search.back().succ.erase(search.back().succ.begin()+x);
252  x--;
253  }
254  }
255 
256  // reverse and then handle them from back to front
257  std::reverse(search.back().succ.begin(), search.back().succ.end());
258  //return false;
259  }
260 
261  if (search.back().status == kGoingAcross)
262  {
263  // no more succ to go down - go up
264  if (search.back().succ.size() == 0)
265  {
266  //printf("Out of successors\n");
267  search.pop_back();
268  return false;
269  }
270 
271  //printf("Taking next successors\n");
272  // going down - generate next successor
273  search.resize(search.size()+1);
274  auto &s = search[search.size()-2];
275  search.back().currState = s.succ.back();
276  search.back().status = kGoingDown;
277  search.back().pathCost = s.pathCost+env->GCost(s.currState, s.succ.back());
278  s.succ.pop_back();
279  return false;
280  }
281  assert(false);
282  return false;
283 
284 }
285 
286 
287 template <class state, class action>
288 bool IncrementalBTS<state, action>::DoSingleSearchStep(std::vector<state> &thePath)
289 {
290  if (flesseq(solutionCost, data.solutionInterval.lowerBound))
291  {
292  thePath = solutionPath;
293  printf("Found solution cost %1.5f\n", solutionCost);
294  return true;
295  }
296 
297  if (!IterationComplete() && data.nodesExpanded < data.workBound)
298  {
299  uint64_t tmp = nodesExpanded;
300  StepIteration();
301  data.nodesExpanded += nodesExpanded-tmp;
302  if (IterationComplete())
303  {
304  currStage = kCompletedFullStage;
305  stage = "EXP Iter Complete";
306  }
307  return false;
308  }
309 
310  // Invariant - iteration complete *or* exceeded work limit
311  // last search completed - set the interval from this search
312  costInterval v;
313  if (data.nodesExpanded >= data.workBound)
314  {
315  v.lowerBound = 0;
316  v.upperBound = sd.f_below;
317  }
318  else if (solutionCost != DBL_MAX && fgreatereq(sd.f_below, solutionCost))
319  {
320  v.lowerBound = solutionCost;
321  v.upperBound = solutionCost;
322  }
323  else {
324  v.lowerBound = sd.f_above;
325  v.upperBound = DBL_MAX;
326  }
327  data.solutionInterval &= v;
328 
329  printf("--> Iteration complete - %" PRId64 " expanded; target [%" PRId64 ", %" PRId64 ")\n", data.nodesExpanded, c1*data.nodeLB, c2*data.nodeLB);
330  // Move to next iteration
331  if (data.nodesExpanded >= c1*data.nodeLB && data.workBound == infiniteWorkBound)
332  {
333  printf("Expanded %" PRId64 " - needed at least %" PRId64 "\n", data.nodesExpanded, c1*data.nodeLB);
334  if (data.solutionInterval.lowerBound == DBL_MAX) // No new nodes in iteration
335  printf("[HIT]--Critical f in [%1.5f, %1.5f]\n", solutionCost, solutionCost);
336  else
337  printf("[HIT]--Critical f in [%1.5f, ∞]\n", data.solutionInterval.lowerBound);
338  data.nodeLB = data.nodesExpanded;
339  data.solutionInterval.upperBound = DBL_MAX;
340  data.nodesExpanded = 0;
341  fEquation = to_string_with_precision(nextBound, 2);
342  SetupIteration(nextBound);
343  return false;
344  }
345 
346  // Check if bounds are equal or whether we fell within the node bounds
347  if (!
348  (fequal(data.solutionInterval.upperBound, data.solutionInterval.lowerBound) ||
349  (data.nodesExpanded >= c1*data.nodeLB && data.nodesExpanded < c2*data.nodeLB)
350  ))
351  {
352  if (data.solutionInterval.upperBound == DBL_MAX)
353  printf(" ]--Critical f in [%1.5f, ∞]\n", data.solutionInterval.lowerBound);
354  else
355  printf(" ]--Critical f in [%1.5f, %1.5f]\n", data.solutionInterval.lowerBound, data.solutionInterval.upperBound);
356 
357  double nextCost;
358  if (data.solutionInterval.upperBound == DBL_MAX)
359  {
360  nextCost = data.solutionInterval.lowerBound+pow(gamma, data.delta);
361  fEquation = to_string_with_precision(data.solutionInterval.lowerBound, 2)+"+"+std::to_string(gamma)+"^"+std::to_string(data.delta)+"="+to_string_with_precision(nextCost, 2);
362  stage = "EXP";
363  currStage = kExponentialSearch;
364  }
365  else {
366  nextCost = (data.solutionInterval.lowerBound+data.solutionInterval.upperBound)/2.0;
367  fEquation = "("+to_string_with_precision(data.solutionInterval.lowerBound, 2)+"+"+ to_string_with_precision(data.solutionInterval.upperBound, 2)+")/2"+"="+to_string_with_precision(nextCost, 2);
368  stage = "BIN";
369  currStage = kBinarySearch;
370  }
371  data.delta += 1;
372  data.workBound = c2*data.nodeLB;
373  SetupIteration(nextCost);
374  printf("-> Starting new iteration with f: %f; node limit: %" PRId64 "\n", nextCost, data.workBound);
375 
376  return false;
377  }
378 
379  if (data.solutionInterval.lowerBound == DBL_MAX)
380  printf("[HIT]--Critical f in [∞, ∞]\n");
381  else
382  printf("[HIT]--Critical f in [%1.5f, ∞]\n", data.solutionInterval.lowerBound);
383  // finished iteration with current bound - ready to start next IBEX/BTS iteration
384  data.nodeLB = std::max(data.nodesExpanded, c1*data.nodeLB);
385  data.solutionInterval.upperBound = DBL_MAX;
386  data.workBound = infiniteWorkBound;
387  data.nodesExpanded = 0;
388  data.delta = 0;
389  fEquation = to_string_with_precision(nextBound, 2);
390  SetupIteration(nextBound);
391  stage = "NEW ITERATION";
392  return false;
393 }
394 
395 template <class state, class action>
396 void IncrementalBTS<state, action>::Draw(Graphics::Display &display) const
397 {
398  for (size_t x = 1; x < search.size(); x++)
399  {
400  env->DrawLine(display, search[x-1].currState, search[x].currState, 10);
401  }
402 }
403 
404 template <class state, class action>
405 void IncrementalBTS<state, action>::OpenGLDraw()
406 {
407  // for (auto x : history)
408  // env->OpenGLDraw(x.first);
409  for (size_t x = 1; x < search.size(); x++)
410  env->GLDrawLine(search[x-1].currState, search[x].currState, 10);
411  // for (int x = 1; x < path.size(); x++)
412  // env->GLDrawLine(path[x-1], path[x]);
413 }
414 
415 
416 #endif /* IncrementalBTS_h */
IncrementalBTS::GetNextFLimit
double GetNextFLimit()
Definition: IncrementalBTS.h:58
IncrementalBTS::IBEXData::nodeLB
uint64_t nodeLB
Definition: IncrementalBTS.h:85
IncrementalBTS::solutionCost
double solutionCost
Definition: IncrementalBTS.h:106
IncrementalBTS::gamma
const uint64_t gamma
Definition: IncrementalBTS.h:62
min
double min(double a, double b)
Definition: FPUtil.h:35
IncrementalBTS::currSearchState::status
kSearchStatus status
Definition: IncrementalBTS.h:101
IncrementalBTS::currStage
IBEXStage currStage
Definition: IncrementalBTS.h:70
fgreatereq
bool fgreatereq(double a, double b)
Definition: FPUtil.h:31
Heuristic
Definition: Heuristic.h:30
IncrementalBTS::searchData
Definition: IncrementalBTS.h:90
IncrementalBTS::infiniteWorkBound
const int infiniteWorkBound
Definition: IncrementalBTS.h:63
IncrementalBTS::SetupIteration
void SetupIteration(double cost)
Definition: IncrementalBTS.h:179
IncrementalBTS::nextBound
double nextBound
Definition: IncrementalBTS.h:119
IncrementalBTS::search
std::vector< currSearchState > search
Definition: IncrementalBTS.h:105
Heuristic.h
IBEX::infiniteWorkBound
const int infiniteWorkBound
Definition: IBEX.h:35
kIDAStarSearch
@ kIDAStarSearch
Definition: IncrementalBTS.h:17
IncrementalBTS::IBEXData::nodesExpanded
uint64_t nodesExpanded
Definition: IncrementalBTS.h:83
IncrementalBTS::solutionPath
std::vector< state > solutionPath
Definition: IncrementalBTS.h:114
IncrementalBTS::newNodesLastIteration
uint64_t newNodesLastIteration
Definition: IncrementalBTS.h:123
IncrementalBTS::kSearchStatus
kSearchStatus
Definition: IncrementalBTS.h:95
IncrementalBTS::kGoingAcross
@ kGoingAcross
Definition: IncrementalBTS.h:97
IncrementalBTS::searchData::f_below
double f_below
Definition: IncrementalBTS.h:91
IncrementalBTS::history
std::vector< std::pair< state, double > > history
Definition: IncrementalBTS.h:113
IncrementalBTS::GetCurrentPath
void GetCurrentPath(std::vector< state > &p) const
Definition: IncrementalBTS.h:55
IncrementalBTS::fEquation
std::string fEquation
Definition: IncrementalBTS.h:69
IncrementalBTS::data
IBEXData data
Definition: IncrementalBTS.h:89
IncrementalBTS::start
state start
Definition: IncrementalBTS.h:115
IncrementalBTS::nodesExpanded
unsigned long nodesExpanded
Definition: IncrementalBTS.h:108
IncrementalBTS::kGoingDown
@ kGoingDown
Definition: IncrementalBTS.h:96
IncrementalBTS::IBEXData::workBound
uint64_t workBound
Definition: IncrementalBTS.h:84
IncrementalBTS::GetNodesTouched
uint64_t GetNodesTouched()
Definition: IncrementalBTS.h:36
IncrementalBTS::nodesTouched
unsigned long nodesTouched
Definition: IncrementalBTS.h:108
IBEXStage
IBEXStage
Definition: IncrementalBTS.h:15
IncrementalBTS::Draw
void Draw(Graphics::Display &display) const
Definition: IncrementalBTS.h:396
to_string_with_precision
std::string to_string_with_precision(const T a_value, const int n=6)
Definition: GLUtil.h:185
IncrementalBTS::InitializeSearch
bool InitializeSearch(SearchEnvironment< state, action > *env, state from, state to, Heuristic< state > *h, std::vector< state > &thePath)
Definition: IncrementalBTS.h:144
IncrementalBTS::h
Heuristic< state > * h
Definition: IncrementalBTS.h:121
IncrementalBTS::IBEXData
Definition: IncrementalBTS.h:82
IncrementalBTS::DoSingleSearchStep
bool DoSingleSearchStep(std::vector< state > &thePath)
Definition: IncrementalBTS.h:288
IncrementalBTS::GetCurrentState
state GetCurrentState() const
Definition: IncrementalBTS.h:46
IncrementalBTS::GetCurrentFLimit
double GetCurrentFLimit()
Definition: IncrementalBTS.h:57
IncrementalBTS::IterationComplete
bool IterationComplete()
Definition: IncrementalBTS.h:107
Graphics::Display
Definition: Graphics.h:146
fless
bool fless(double a, double b)
Definition: FPUtil.h:28
IncrementalBTS::ResetNodeCount
void ResetNodeCount()
Definition: IncrementalBTS.h:37
kBinarySearch
@ kBinarySearch
Definition: IncrementalBTS.h:19
IncrementalBTS::costInterval
Definition: IncrementalBTS.h:72
IncrementalBTS::GetIterationNodesExpanded
uint64_t GetIterationNodesExpanded()
Definition: IncrementalBTS.h:35
IncrementalBTS::costInterval::lowerBound
double lowerBound
Definition: IncrementalBTS.h:73
IncrementalBTS::env
SearchEnvironment< state, action > * env
Definition: IncrementalBTS.h:120
Heuristic::HCost
virtual double HCost(const state &a, const state &b) const
Definition: Heuristic.h:73
IncrementalBTS::currSearchState
Definition: IncrementalBTS.h:99
IncrementalBTS::stage
std::string stage
Definition: IncrementalBTS.h:68
IncrementalBTS::goal
state goal
Definition: IncrementalBTS.h:115
IncrementalBTS::GetNodesExpanded
uint64_t GetNodesExpanded()
Definition: IncrementalBTS.h:34
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
IncrementalBTS::GetNodeInterval
void GetNodeInterval(uint64_t &lower, uint64_t &upper)
Definition: IncrementalBTS.h:66
IncrementalBTS::IncrementalBTS
IncrementalBTS(double initialBound=0)
Definition: IncrementalBTS.h:25
IncrementalBTS::c1
const uint64_t c1
Definition: IncrementalBTS.h:60
IncrementalBTS::sd
searchData sd
Definition: IncrementalBTS.h:94
IncrementalBTS::previousBound
double previousBound
Definition: IncrementalBTS.h:116
IncrementalBTS::searchData::f_above
double f_above
Definition: IncrementalBTS.h:92
IncrementalBTS::GetNewNodesLastIteration
uint64_t GetNewNodesLastIteration()
Definition: IncrementalBTS.h:59
IncrementalBTS::bound
double bound
Definition: IncrementalBTS.h:117
IncrementalBTS::newNodeCount
uint64_t newNodeCount
Definition: IncrementalBTS.h:123
IncrementalBTS::OpenGLDraw
void OpenGLDraw()
Definition: IncrementalBTS.h:405
IncrementalBTS::IBEXData::delta
int delta
Definition: IncrementalBTS.h:87
IncrementalBTS::c2
const uint64_t c2
Definition: IncrementalBTS.h:61
IncrementalBTS::costInterval::upperBound
double upperBound
Definition: IncrementalBTS.h:74
IncrementalBTS::GetGlobalCostInterval
void GetGlobalCostInterval(double &lower, double &upper)
Definition: IncrementalBTS.h:64
IncrementalBTS::currSearchState::succ
std::vector< state > succ
Definition: IncrementalBTS.h:103
IncrementalBTS::IBEXData::solutionInterval
costInterval solutionInterval
Definition: IncrementalBTS.h:86
IncrementalBTS::Reset
void Reset()
Definition: IncrementalBTS.h:42
kExponentialSearch
@ kExponentialSearch
Definition: IncrementalBTS.h:18
IncrementalBTS::GetPath
void GetPath(SearchEnvironment< state, action > *env, state from, state to, Heuristic< state > *h, std::vector< state > &thePath)
Definition: IncrementalBTS.h:127
kCompletedFullStage
@ kCompletedFullStage
Definition: IncrementalBTS.h:16
IncrementalBTS::succ
std::vector< state > succ
Definition: IncrementalBTS.h:122
fequal
bool fequal(double a, double b, double tolerance=TOLERANCE)
Definition: FPUtil.h:32
IncrementalBTS::currSearchState::currState
state currState
Definition: IncrementalBTS.h:100
IncrementalBTS::StepIteration
bool StepIteration()
Definition: IncrementalBTS.h:197
SearchEnvironment
Definition: SearchEnvironment.h:30
IncrementalBTS::initialBound
double initialBound
Definition: IncrementalBTS.h:118
flesseq
bool flesseq(double a, double b)
Definition: FPUtil.h:30
IncrementalBTS::currSearchState::pathCost
double pathCost
Definition: IncrementalBTS.h:102
IncrementalBTS
Definition: IncrementalBTS.h:23
IncrementalBTS::costInterval::operator&=
costInterval & operator&=(const costInterval &i)
Definition: IncrementalBTS.h:75
Generated by   doxygen 1.8.17