HOG2
EBSearch.h
Go to the documentation of this file.
1 //
2 // BID.h
3 // hog2 glut
4 //
5 // Created by Nathan Sturtevant on 2/28/18.
6 // Copyright © 2018 University of Denver. All rights reserved.
7 //
8 
9 #ifndef BID_h
10 #define BID_h
11 
12 #include "vectorCache.h"
13 #include "AStarOpenClosed.h"
14 
15 template <class state>
16 struct BFHSCompare {
17  bool operator()(const AStarOpenClosedData<state> &i1, const AStarOpenClosedData<state> &i2) const
18  {
19  return (fgreater(i1.g, i2.g));
20  }
21 };
22 
23 
24 template <class state, class action, class environment, bool DFS = true>
25 class EBSearch {
26 public:
27  EBSearch(uint64_t minGrow, uint64_t maxGrow, uint64_t expEpsilon, uint64_t scale = 1)
28  :c1(minGrow), c2(maxGrow), initialGap(expEpsilon), costScale(scale) {}
29  void GetPath(environment *env, state from, state to,
30  std::vector<action> &thePath);
31  void GetPath(environment *env, Heuristic<state> *heuristic, state from, state to,
32  std::vector<action> &thePath);
33  void RedoMinWork();
34 
35  uint64_t GetNodesExpanded() { return totalNodesExpanded; }
36  uint64_t GetNodesTouched() { return totalNodesTouched; }
37  void ResetNodeCount() { totalNodesExpanded = totalNodesTouched = 0; }
38 private:
39  struct searchData {
40  uint64_t f;
41  uint64_t nextF;
42  uint64_t failedF;
43  uint64_t nodes;
44  };
45  EBSearch<state, action, environment, DFS>::searchData LowLevelSearch(uint64_t costLimit, uint64_t nodeLimit);
46  EBSearch<state, action, environment, DFS>::searchData BinarySearch(searchData d, uint64_t nodeLimit);
47  EBSearch<state, action, environment, DFS>::searchData ExponentialSearch(const searchData &d, uint64_t nodeLimit);
48 
49  // Functions for DF Search
50  uint64_t GCost(const state &s1, const state &s2)
51  { return env->GCost(s1, s2)*costScale; }
52  uint64_t GCost(const state &s, const action &a)
53  { return env->GCost(s, a)*costScale; }
54  uint64_t HCost(const state &s)
55  { return h->HCost(s, goal)*costScale; }
56  EBSearch<state, action, environment, DFS>::searchData DFBNB(uint64_t costLimit, uint64_t nodeLimit);
57  EBSearch<state, action, environment, DFS>::searchData DFBNBHelper(state &currState, uint64_t pathCost, uint64_t costLimit,
58  searchData &sd, uint64_t nodeLimit, action forbidden);
59 
60  // Functions for BFHS
61  EBSearch<state, action, environment, DFS>::searchData BFHS(uint64_t costLimit, uint64_t nodeLimit);
62  void ExtractPathToStartFromID(uint64_t node, std::vector<state> &thePath);
63 
64 
65  uint64_t costScale;
66 
67  uint64_t totalNodesExpanded, totalNodesTouched;
68  Heuristic<state> *h;
69  environment *env;
70  std::vector<action> solutionPath, currPath;
71  uint64_t solutionCost;
72  vectorCache<action> actCache;
73  state start, goal;
74  uint64_t c1, c2;
75  uint64_t initialGap;
76 
77  // Data for BFHS
78  AStarOpenClosed<state, BFHSCompare<state>> q;
79  std::vector<state> neighbors;
80  std::vector<uint64_t> neighborID;
81  std::vector<uint64_t> edgeCosts;
82  std::vector<dataLocation> neighborLoc;
83  std::vector<state> solutionStates;
84 };
85 
86 template <class state, class action, class environment, bool DFS>
87 void EBSearch<state, action, environment, DFS>::GetPath(environment *env, state from, state to,
88  std::vector<action> &thePath)
89 {
90  GetPath(env, env, from, to, thePath);
91 }
92 
93 template <class state, class action, class environment, bool DFS>
94 void EBSearch<state, action, environment, DFS>::GetPath(environment *env, Heuristic<state> *heuristic, state from, state to,
95  std::vector<action> &thePath)
96 {
97  this->env = env;
98  h = heuristic;
99  start = from;
100  goal = to;
101  solutionPath.clear();
102  solutionCost = -1ull;
103  ResetNodeCount();
104 
105  printf("EBIS: Baseline search: f: %llu\n", HCost(from));
106  searchData base = LowLevelSearch(HCost(from), -1);
107  while (solutionCost > base.f)
108  {
109  printf("EBIS: First search: f: %1llu target: (%llu, %llu]\n",
110  base.nextF, uint64_t(c1*base.nodes), uint64_t(c2*base.nodes));
111  searchData curr = LowLevelSearch(base.nextF, -1);
112  if (solutionCost <= base.nextF)
113  break;
114 
115  // Didn't reach the node expansions bound
116  if (curr.nodes < uint64_t(c1*base.nodes))
117  {
118  printf("EBIS: %llu under target %llu, initiate EXP search\n", curr.nodes, uint64_t(c1*base.nodes));
119  curr = ExponentialSearch(curr, base.nodes);
120 
121  // Overshot the node expansions bound
122  if (curr.nodes >= uint64_t(c2*base.nodes))
123  {
124  printf("EBIS: %llu over target %llu, initiate BIN search\n", curr.nodes, uint64_t(c2*base.nodes));
125  curr = BinarySearch(curr, base.nodes);
126  }
127  }
128  if (solutionCost <= curr.f)
129  break;
130  base = curr;
131  }
132  thePath = solutionPath;
133  printf("Found solution cost %llu\n", solutionCost);
134 }
135 
136 template <class state, class action, class environment, bool DFS>
137 typename EBSearch<state, action, environment, DFS>::searchData EBSearch<state, action, environment, DFS>::ExponentialSearch(const searchData &d, uint64_t nodeLimit)
138 {
139  uint64_t lowNodes = c1*nodeLimit;
140  uint64_t highNodes = c2*nodeLimit;
141  uint64_t delta = std::max(d.nextF - d.f, initialGap);
142  int i = 0;
143  searchData lastSuccess = d;
144  while (true)
145  {
146  uint64_t bound = d.f + delta*pow(2, i);
147  searchData curr = LowLevelSearch(bound, highNodes);
148  if (solutionCost <= bound && curr.nodes < highNodes)
149  {
150  printf("***EBIS:->EXP: Solution proven below window; done\n");
151  curr.f = solutionCost;
152  return curr;
153  }
154 
155  if (lowNodes <= curr.nodes && curr.nodes < highNodes)
156  {
157  printf("EBIS:->EXP: in node window [%llu <= %llu < %llu]\n", lowNodes, curr.nodes, highNodes);
158  return curr;
159  }
160  else if (curr.nodes >= highNodes)
161  {
162  printf("EBIS:->EXP: over node window\n");
163  lastSuccess.nodes = curr.nodes;
164  lastSuccess.failedF = bound;
165  return lastSuccess;
166  }
167  else {
168  printf("EBIS:->EXP: below node window\n");
169  lastSuccess = curr;
170  }
171  i++;
172  if (d.f + delta*pow(2, i) < curr.nextF)
173  printf("EBIS:->EXP: delta too small, increasing growth past nextf\n");
174  while (d.f + delta*pow(2, i) < curr.nextF)
175  {
176  i++;
177  }
178  }
179 }
180 
181 template <class state, class action, class environment, bool DFS>
182 typename EBSearch<state, action, environment, DFS>::searchData EBSearch<state, action, environment, DFS>::BinarySearch(searchData d, uint64_t nodeLimit)
183 {
184  uint64_t lowNodes = c1*nodeLimit;
185  uint64_t highNodes = c2*nodeLimit;
186  uint64_t middlef = (d.failedF + d.f)/2.0;
187  searchData curr;
188  if (middlef <= d.nextF)
189  {
190  middlef = d.nextF;
191  curr = LowLevelSearch(middlef, -1);
192  if (solutionCost <= middlef)
193  curr.f = middlef;
194  if (curr.nodes >= lowNodes)
195  return curr;
196  }
197  else {
198  curr = LowLevelSearch(middlef, highNodes);
199  }
200  // found and proved solution
201  if (solutionCost <= middlef && curr.nodes < highNodes)
202  {
203  printf("***EBIS:->BIN: Solution proven below window; done\n");
204  curr.f = middlef;
205  return curr;
206  }
207 
208  if (lowNodes <= curr.nodes && curr.nodes < highNodes)
209  {
210  printf("EBIS:->BIN: in node window [%llu <= %llu < %llu]\n", lowNodes, curr.nodes, highNodes);
211  return curr;
212  }
213  else if (curr.nodes >= highNodes)
214  {
215  printf("EBIS:->BIN: over node window\n");
216  d.failedF = std::min(middlef, solutionCost);
217  return BinarySearch(d, nodeLimit);
218  }
219  else {
220  printf("EBIS:->BIN: below node window\n");
221  curr.failedF = d.failedF;
222  return BinarySearch(curr, nodeLimit);
223  }
224 }
225 
226 template <class state, class action, class environment, bool DFS>
227 typename EBSearch<state, action, environment, DFS>::searchData EBSearch<state, action, environment, DFS>::DFBNB(uint64_t costLimit, uint64_t nodeLimit)
228 {
229  state currState = start;
230  if (nodeLimit != -1)
231  printf(" --+DFBnB f: %llu; nodes: %llu; ", costLimit, nodeLimit);
232  else
233  printf(" --+DFBnB f: %llu; nodes: ∞; ", costLimit);
234  currPath.clear();
235  searchData sd;
236  sd.nextF = -1ull;
237  sd.f = 0;
238  sd.nodes = 0;
239  action a;
240  sd = DFBNBHelper(currState, 0, costLimit, sd, nodeLimit, a);
241  totalNodesExpanded += sd.nodes;
242  // TODO: In practice we should set the nextF to be sd.f
243  // Need to test this.
244  if (sd.nextF == -1ull) // so few nodes expanded we didn't find the next bound
245  {
246  printf(" (oops) ");
247  sd = DFBNBHelper(currState, 0, costLimit, sd, -1ull, a);
248  totalNodesExpanded += sd.nodes;
249  }
250  printf("%llu (new) %llu (total), maxf: %llu, nextf: %llu\n", sd.nodes, totalNodesExpanded, sd.f, sd.nextF);
251  return sd;
252 }
253 
254 template <class state, class action, class environment, bool DFS>
255 typename EBSearch<state, action, environment, DFS>::searchData EBSearch<state, action, environment, DFS>::DFBNBHelper(state &currState, uint64_t pathCost, uint64_t costLimit,
256  searchData &sd, uint64_t nodeLimit, action forbidden)
257 {
258  uint64_t currF = pathCost+HCost(currState);
259 // printf("-------->%f [%f]\n", currF, pathCost);
260  if (fgreater(currF, costLimit) || fgreatereq(currF, solutionCost))
261  {
262  sd.nextF = std::min(sd.nextF, currF);
263  return sd;
264  }
265  else {
266  sd.f = std::max(currF, sd.f);
267  }
268 
269  if (sd.nodes >= nodeLimit)
270  return sd;
271  if (env->GoalTest(currState, goal))
272  {
273  if (fless(currF, solutionCost))
274  {
275  solutionPath = currPath;
276  solutionCost = currF;
277  }
278  return sd;
279  }
280 
281  std::vector<action> &acts = *actCache.getItem();
282  env->GetActions(currState, acts);
283  sd.nodes++;
284  totalNodesTouched+=acts.size();
285 
286  for (size_t x = 0; x < acts.size(); x++)
287  {
288  if (acts[x] == forbidden && currPath.size() > 0)
289  continue;
290  uint64_t edgeCost = GCost(currState, acts[x]);
291  env->ApplyAction(currState, acts[x]);
292  currPath.push_back(acts[x]);
293  action rev = acts[x];
294  env->InvertAction(rev);
295  sd = DFBNBHelper(currState, pathCost+edgeCost, costLimit, sd, nodeLimit, rev);
296  currPath.pop_back();
297  env->UndoAction(currState, acts[x]);
298  }
299  actCache.returnItem(&acts);
300  return sd;
301 }
302 
303 template <class state, class action, class environment, bool DFS>
304 void EBSearch<state, action, environment, DFS>::RedoMinWork()
305 {
306  ResetNodeCount();
307  printf("EBIS Validation:\n");
308  LowLevelSearch(solutionCost, -1);
309 }
310 
311 template <class state, class action, class environment, bool DFS>
312 typename EBSearch<state, action, environment, DFS>::searchData EBSearch<state, action, environment, DFS>::BFHS(uint64_t costLimit, uint64_t nodeLimit)
313 {
314  if (nodeLimit == -1ull && costLimit == -1ull)
315  printf(" --+BFHS f: ∞; nodes: ∞; ");
316  else if (nodeLimit == -1)
317  printf(" --+BFHS f: %llu; nodes: ∞; ", costLimit);
318  else
319  printf(" --+BFHS f: %llu; nodes: %llu; ", costLimit, nodeLimit);
320 
321  q.Reset(env->GetMaxHash());
322  // put start in open
323  q.AddOpenNode(start, env->GetStateHash(start), 0.0, 0.0, (uint64_t)0);
324  uint64_t nextCost = -1, maxFExpanded = 0;
325  uint64_t nodesExpanded = 0;
326  while (nodesExpanded < nodeLimit && q.OpenSize() > 0)
327  {
328  // expand by low g until
329  // (1) we find the goal
330  // (2) we hit the node limit
331  // (3) we exhaust all states
332  uint64_t nodeid = q.Close();
333  nodesExpanded++;
334  totalNodesExpanded++;
335 
336  maxFExpanded = std::max(maxFExpanded, static_cast<uint64_t>(q.Lookup(nodeid).g+HCost(q.Lookup(nodeid).data)));
337 
338  if (env->GoalTest(q.Lookup(nodeid).data, goal))
339  {
340  solutionCost = q.Lookup(nodeid).g;
341  ExtractPathToStartFromID(nodeid, solutionStates);
342  // Path is backwards - reverse
343  reverse(solutionStates.begin(), solutionStates.end());
344  // f, nextF, failedF, nodes
345  if(solutionStates.size() > 0) {
346  for (size_t x = 0; x < solutionStates.size()-1; x++)
347  {
348  solutionPath.push_back(env->GetAction(solutionStates[x], solutionStates[x+1]));
349  }
350  }
351  return {static_cast<uint64_t>(q.Lookup(nodeid).g), static_cast<uint64_t>(q.Lookup(nodeid).g), -1ull, nodesExpanded};
352  }
353 
354  neighbors.resize(0);
355  edgeCosts.resize(0);
356  neighborID.resize(0);
357  neighborLoc.resize(0);
358 
359  // std::cout << "Expanding: " << q.Lookup(nodeid).data << " with f:";
360  // std::cout << q.Lookup(nodeid).g << std::endl;
361 
362  env->GetSuccessors(q.Lookup(nodeid).data, neighbors);
363 
364  // 1. load all the children
365  for (size_t x = 0; x < neighbors.size(); x++)
366  {
367  uint64_t theID;
368  neighborLoc.push_back(q.Lookup(env->GetStateHash(neighbors[x]), theID));
369  neighborID.push_back(theID);
370  edgeCosts.push_back(GCost(q.Lookup(nodeid).data, neighbors[x]));
371  }
372 
373  // iterate again updating costs and writing out to memory
374  for (size_t x = 0; x < neighbors.size(); x++)
375  {
376  totalNodesTouched++;
377 
378  switch (neighborLoc[x])
379  {
380  case kClosedList:
381  break;
382  case kOpenList:
383  if (fless(q.Lookup(nodeid).g+edgeCosts[x], q.Lookup(neighborID[x]).g))
384  {
385  q.Lookup(neighborID[x]).parentID = nodeid;
386  q.Lookup(neighborID[x]).g = q.Lookup(nodeid).g+edgeCosts[x];
387  q.KeyChanged(neighborID[x]);
388  }
389  break;
390  case kNotFound:
391  {
392  uint64_t fcost = q.Lookup(nodeid).g+edgeCosts[x]+HCost(neighbors[x]);
393  // TODO: don't add states with g == best solution when one exists
394  if (fcost > costLimit)
395  {
396  nextCost = std::min(nextCost, fcost);
397  }
398  else {
399  q.AddOpenNode(neighbors[x],
400  env->GetStateHash(neighbors[x]),
401  q.Lookup(nodeid).g+edgeCosts[x],
402  0.0,
403  nodeid);
404  }
405  }
406  }
407  }
408  }
409  // f, nextF, failedF, nodes
410 
411  if (nextCost == -1ull)
412  printf("%llu (new) %llu (total), maxf: %llu, nextf: ∞\n", nodesExpanded, totalNodesExpanded, maxFExpanded);
413  else
414  printf("%llu (new) %llu (total), maxf: %llu, nextf: %llu\n", nodesExpanded, totalNodesExpanded, maxFExpanded, nextCost);
415 
416  // Exceeded node limit
417  if (nodesExpanded == nodeLimit)
418  {
419  return {maxFExpanded, nextCost, maxFExpanded, nodesExpanded};
420  }
421  // expanded all nodes in limit
422  if (q.OpenSize()==0)
423  {
424  return {maxFExpanded, nextCost, -1ull, nodesExpanded};
425  }
426  assert(!"Shouldn't get here");
427 }
428 
429 template <class state, class action,class environment,bool DFS>
430 void EBSearch<state, action, environment, DFS>::ExtractPathToStartFromID(uint64_t node,
431  std::vector<state> &thePath)
432 {
433  thePath.clear();
434  do {
435  thePath.push_back(q.Lookup(node).data);
436  node = q.Lookup(node).parentID;
437  } while (q.Lookup(node).parentID != node);
438  thePath.push_back(q.Lookup(node).data);
439 }
440 
441 
442 template <class state, class action, class environment, bool DFS>
443 typename EBSearch<state, action, environment, DFS>::searchData EBSearch<state, action, environment, DFS>::LowLevelSearch(uint64_t costLimit, uint64_t nodeLimit)
444 {
445  if (DFS)
446  return DFBNB(costLimit, nodeLimit);
447  else
448  return BFHS(costLimit, nodeLimit);
449 }
450 #endif /* BID_h */
EBSearch::DFBNB
EBSearch< state, action, environment, DFS >::searchData DFBNB(uint64_t costLimit, uint64_t nodeLimit)
Definition: EBSearch.h:227
EBSearch::GetNodesExpanded
uint64_t GetNodesExpanded()
Definition: EBSearch.h:35
EBSearch::LowLevelSearch
EBSearch< state, action, environment, DFS >::searchData LowLevelSearch(uint64_t costLimit, uint64_t nodeLimit)
Definition: EBSearch.h:443
min
double min(double a, double b)
Definition: FPUtil.h:35
EBSearch::searchData::failedF
uint64_t failedF
Definition: EBSearch.h:42
fgreatereq
bool fgreatereq(double a, double b)
Definition: FPUtil.h:31
Heuristic
Definition: Heuristic.h:30
EBSearch::solutionStates
std::vector< state > solutionStates
Definition: EBSearch.h:83
AStarOpenClosed.h
d
mcData d[]
Definition: MotionCaptureMovement.cpp:21
EBSearch::initialGap
uint64_t initialGap
Definition: EBSearch.h:75
EBSearch::ResetNodeCount
void ResetNodeCount()
Definition: EBSearch.h:37
EBSearch::totalNodesExpanded
uint64_t totalNodesExpanded
Definition: EBSearch.h:67
vectorCache.h
BFHSCompare::operator()
bool operator()(const AStarOpenClosedData< state > &i1, const AStarOpenClosedData< state > &i2) const
Definition: EBSearch.h:17
EBSearch::goal
state goal
Definition: EBSearch.h:73
EBSearch::GetNodesTouched
uint64_t GetNodesTouched()
Definition: EBSearch.h:36
AStarOpenClosedData::g
double g
Definition: AStarOpenClosed.h:64
EBSearch::env
environment * env
Definition: EBSearch.h:69
EBSearch::h
Heuristic< state > * h
Definition: EBSearch.h:68
EBSearch::c2
uint64_t c2
Definition: EBSearch.h:74
EBSearch::edgeCosts
std::vector< uint64_t > edgeCosts
Definition: EBSearch.h:81
EBSearch::BinarySearch
EBSearch< state, action, environment, DFS >::searchData BinarySearch(searchData d, uint64_t nodeLimit)
Definition: EBSearch.h:182
AStarOpenClosed
Definition: AStarOpenClosed.h:74
fless
bool fless(double a, double b)
Definition: FPUtil.h:28
EBSearch::BFHS
EBSearch< state, action, environment, DFS >::searchData BFHS(uint64_t costLimit, uint64_t nodeLimit)
Definition: EBSearch.h:312
EBSearch::GCost
uint64_t GCost(const state &s1, const state &s2)
Definition: EBSearch.h:50
AStarOpenClosedData
Definition: AStarOpenClosed.h:52
EBSearch::searchData::nextF
uint64_t nextF
Definition: EBSearch.h:41
DFS
Definition: DFS.h:19
EBSearch::searchData::f
uint64_t f
Definition: EBSearch.h:40
EBSearch::searchData::nodes
uint64_t nodes
Definition: EBSearch.h:43
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
EBSearch::HCost
uint64_t HCost(const state &s)
Definition: EBSearch.h:54
EBSearch::searchData
Definition: EBSearch.h:39
EBSearch::DFBNBHelper
EBSearch< state, action, environment, DFS >::searchData DFBNBHelper(state &currState, uint64_t pathCost, uint64_t costLimit, searchData &sd, uint64_t nodeLimit, action forbidden)
Definition: EBSearch.h:255
EBSearch::c1
uint64_t c1
Definition: EBSearch.h:74
vectorCache< action >
EBSearch::costScale
uint64_t costScale
Definition: EBSearch.h:65
kOpenList
@ kOpenList
Definition: AStarOpenClosed.h:28
EBSearch::neighbors
std::vector< state > neighbors
Definition: EBSearch.h:79
EBSearch::neighborID
std::vector< uint64_t > neighborID
Definition: EBSearch.h:80
EBSearch
Definition: EBSearch.h:25
EBSearch::solutionPath
std::vector< action > solutionPath
Definition: EBSearch.h:70
EBSearch::actCache
vectorCache< action > actCache
Definition: EBSearch.h:72
BFHSCompare
Definition: EBSearch.h:16
EBSearch::currPath
std::vector< action > currPath
Definition: EBSearch.h:70
EBSearch::q
AStarOpenClosed< state, BFHSCompare< state > > q
Definition: EBSearch.h:78
EBSearch::GetPath
void GetPath(environment *env, state from, state to, std::vector< action > &thePath)
Definition: EBSearch.h:87
EBSearch::totalNodesTouched
uint64_t totalNodesTouched
Definition: EBSearch.h:67
EBSearch::neighborLoc
std::vector< dataLocation > neighborLoc
Definition: EBSearch.h:82
EBSearch::solutionCost
uint64_t solutionCost
Definition: EBSearch.h:71
kNotFound
@ kNotFound
Definition: AStarOpenClosed.h:30
EBSearch::ExtractPathToStartFromID
void ExtractPathToStartFromID(uint64_t node, std::vector< state > &thePath)
Definition: EBSearch.h:430
EBSearch::GCost
uint64_t GCost(const state &s, const action &a)
Definition: EBSearch.h:52
EBSearch::start
state start
Definition: EBSearch.h:73
EBSearch::RedoMinWork
void RedoMinWork()
Definition: EBSearch.h:304
EBSearch::ExponentialSearch
EBSearch< state, action, environment, DFS >::searchData ExponentialSearch(const searchData &d, uint64_t nodeLimit)
Definition: EBSearch.h:137
EBSearch::EBSearch
EBSearch(uint64_t minGrow, uint64_t maxGrow, uint64_t expEpsilon, uint64_t scale=1)
Definition: EBSearch.h:27
kClosedList
@ kClosedList
Definition: AStarOpenClosed.h:29
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
Generated by   doxygen 1.8.17