LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
logic_engine.h
Go to the documentation of this file.
1 
7 namespace ompl_lemur
8 {
9 
10 // custom propositional logic engine
11 // over a finite set of propositional variables (aka constants)
12 // assignment means partial assignment
14 {
15 public:
16  std::size_t num_vars;
17  bool initialized;
18  LogicEngine(std::size_t num_vars):
19  num_vars(num_vars),
20  initialized(false)
21  {
22  }
23 
24  struct Assignment
25  {
26  std::vector<bool> var_mask; // true == is assigned
27  std::vector<bool> values;
28  bool operator==(const Assignment & rhs) const
29  {
30  if (var_mask != rhs.var_mask)
31  return false;
32  if (values.size() != rhs.values.size())
33  return false;
34  for (std::size_t i=0; i<values.size(); i++)
35  {
36  if (!var_mask[i])
37  continue;
38  if (values[i] != rhs.values[i])
39  return false;
40  }
41  return true;
42  }
43  };
44 
45  struct VarVal
46  {
47  std::size_t var;
48  bool value;
49  };
51  {
52  VarVal new_varval;
53  Assignment result;
54  };
55  struct SearchVertex
56  {
57  double cost_sofar;
58  AssignmentDelta delta;
59  std::vector<bool> row_is_consistent;
60  std::size_t parent_idx;
61  };
62 
63  Assignment empty_assignment()
64  {
65  Assignment pa;
66  pa.var_mask.resize(num_vars, false);
67  pa.values.resize(num_vars, false);
68  return pa;
69  }
70 
71  // compound sentences (permanent)
72  // A1 n A2 n A3 => C
74  {
75  std::vector<std::size_t> antecedents;
76  std::size_t consequent;
77  };
78  std::vector<ConjunctionImplication> conjunction_implications;
79  void add_conjunction_implication(
80  std::vector<std::size_t> antecedents, std::size_t consequent)
81  {
83  x.antecedents = antecedents;
84  x.consequent = consequent;
85  conjunction_implications.push_back(x);
86  }
87 
88  std::vector< std::vector<bool> > truth_table;
89 
90  bool initialize()
91  {
92  if (initialized)
93  return false;
94 
95  // generate all possible truth table rows
96  std::vector<bool> row(num_vars, false);
97  for (;;)
98  {
99  // skip invalid truth table rows
100  std::vector<ConjunctionImplication>::iterator implit;
101  for (implit=conjunction_implications.begin();
102  implit!=conjunction_implications.end(); implit++)
103  {
104  if (row[implit->consequent])
105  continue;
106  std::vector<std::size_t>::iterator anteit;
107  for (anteit=implit->antecedents.begin();
108  anteit!=implit->antecedents.end(); anteit++)
109  if (!row[*anteit])
110  break;
111  if (anteit!=implit->antecedents.end())
112  continue;
113  break;
114  }
115  if (implit==conjunction_implications.end())
116  truth_table.push_back(row);
117 
118  // increment
119  std::size_t iconst;
120  for (iconst=0; iconst<num_vars; iconst++)
121  {
122  row[iconst] = !row[iconst];
123  if (row[iconst])
124  break;
125  }
126  if (!(iconst<num_vars))
127  break;
128  }
129 
130 
131  initialized = true;
132  return true;
133  }
134 
135  void cheapest(const Assignment & current,
136  const VarVal & target_assignment,
137  const std::vector<double> & var_costs,
138  std::vector<AssignmentDelta> & deltas)
139  {
140  // open and closed list
141  std::vector<SearchVertex> generated;
143 
144  SearchVertex v_start;
145  v_start.cost_sofar = 0.0;
146  v_start.delta.result = current;
147  v_start.row_is_consistent.resize(truth_table.size());
148  v_start.parent_idx = 0;
149  for (std::size_t irow=0; irow<truth_table.size(); irow++)
150  {
151  // is this row inconsistent?
152  std::size_t ic = 0;
153  for (; ic<num_vars; ic++)
154  {
155  if (!current.var_mask[ic])
156  continue;
157  if (current.values[ic] != truth_table[irow][ic])
158  break;
159  }
160  if (ic<num_vars)
161  v_start.row_is_consistent[irow] = false;
162  else
163  v_start.row_is_consistent[irow] = true;
164  }
165  heap.insert(generated.size(), v_start.cost_sofar);
166  generated.push_back(v_start);
167 
168  std::size_t top_idx;
169  for (;;)
170  {
171  // grab min from heap
172  if (!heap.size())
173  {
174  fprintf(stderr,"error, no path found!\n");
175  abort();
176  }
177  top_idx = heap.top_idx();
178  SearchVertex v_top = generated[top_idx];
179  heap.remove_min();
180 
181  // is it the goal?
182  if (v_top.delta.result.var_mask[target_assignment.var])
183  {
184  if (v_top.delta.result.values[target_assignment.var]
185  == target_assignment.value)
186  break;
187  continue; // well this definitely wont work
188  }
189 
190  // generate successors
191  for (std::size_t ic_new=0; ic_new<num_vars; ic_new++)
192  {
193  if (v_top.delta.result.var_mask[ic_new])
194  continue;
195  // what if we did this assignment?
196  for (int valnum=0; valnum<2; valnum++)
197  {
198  bool value = valnum ? true : false;
199 
200  SearchVertex v_succ = v_top;
201  v_succ.delta.new_varval.var = ic_new;
202  v_succ.delta.new_varval.value = value;
203  v_succ.cost_sofar += var_costs[ic_new];
204  v_succ.parent_idx = top_idx;
205 
206  // remove inconsistent rows
207  for (std::size_t irow=0; irow<truth_table.size(); irow++)
208  {
209  if (!v_succ.row_is_consistent[irow])
210  continue;
211  if (truth_table[irow][ic_new] != value)
212  v_succ.row_is_consistent[irow] = false;
213  }
214  // find first consistent row
215  // if they're all inconsistent, then we're done
216  std::size_t irow_first;
217  for (irow_first=0; irow_first<truth_table.size(); irow_first++)
218  if (v_succ.row_is_consistent[irow_first])
219  break;
220  if (!(irow_first<truth_table.size()))
221  continue;
222 
223  // determine new (more specific) implied partial assignment
224  v_succ.delta.result.var_mask[ic_new] = true;
225  v_succ.delta.result.values[ic_new] = value;
226  for (std::size_t ic=0; ic<num_vars; ic++)
227  {
228  if (v_succ.delta.result.var_mask[ic])
229  continue;
230  bool candidate_value = truth_table[irow_first][ic];
231  std::size_t irow_different;
232  for (irow_different=irow_first+1; irow_different<truth_table.size(); irow_different++)
233  {
234  if (!v_succ.row_is_consistent[irow_different])
235  continue;
236  if (truth_table[irow_different][ic] != candidate_value)
237  break;
238  }
239  if (irow_different<truth_table.size())
240  continue;
241  v_succ.delta.result.var_mask[ic] = true;
242  v_succ.delta.result.values[ic] = candidate_value;
243  }
244 
245  // does this partial assignment already exist?
246  std::size_t gi;
247  for (gi=0; gi<generated.size(); gi++)
248  if (v_succ.delta.result == generated[gi].delta.result)
249  break;
250  if (gi<generated.size()) // already there!
251  {
252  if (v_succ.cost_sofar < generated[gi].cost_sofar)
253  {
254  generated[gi].cost_sofar = v_succ.cost_sofar;
255  generated[gi].parent_idx = top_idx;
256  heap.update(gi, v_succ.cost_sofar);
257  }
258  }
259  else
260  {
261  heap.insert(generated.size(), v_succ.cost_sofar);
262  generated.push_back(v_succ);
263  }
264  }
265  }
266  }
267 
268  std::vector<std::size_t> path_idxs;
269  path_idxs.push_back(top_idx);
270  while (path_idxs.back())
271  path_idxs.push_back(generated[path_idxs.back()].parent_idx);
272  std::reverse(path_idxs.begin(), path_idxs.end());
273 
274  for (std::size_t ip=1; ip<path_idxs.size(); ip++)
275  deltas.push_back(generated[path_idxs[ip]].delta);
276  }
277 
278 };
279 
280 } // ompl_lemur
Definition: logic_engine.h:55
Definition: logic_engine.h:50
Definition: logic_engine.h:13
Definition: logic_engine.h:24
Definition: logic_engine.h:45