dp.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
16 #ifndef __DP_H__
17 #define __DP_H__
18 
19 #include <iostream>
20 using namespace std;
21 
22 class dpQueue;
23 class dpMain;
24 
29 class dpNode
30 {
31  friend class dpQueue;
32  friend class dpMain;
33 public:
34  dpNode() {
35  cost = 0.0;
36  astar_cost = 0.0;
37  total_cost = 0.0;
38  dp_main = 0;
39  parent = 0;
40  child = 0;
41  brother = 0;
42  queue = 0;
43  id = -1;
44  depth = 0;
45  active = true;
46  }
47  virtual ~dpNode() {
48  if(brother) delete brother;
49  if(child) delete child;
50  }
51 
53  return parent;
54  }
56  return brother;
57  }
59  return child;
60  }
61 
62  int Depth(dpNode* target_p = 0) {
63  int ret;
64  dpNode* p;
65  for(p=this, ret=0; p && p!=target_p; p=p->parent, ret++);
66  if(target_p) ret++;
67  return ret;
68  }
69  dpNode* GetParent(int id) {
70  int i;
71  dpNode* cur;
72  for(i=0, cur=this; cur && i<id; i++, cur=cur->parent);
73  return cur;
74  }
75 
76  double TotalCost() {
77  return total_cost;
78  }
79  double Cost() {
80  return cost;
81  }
82  double TotalAstarCost() {
83  return total_astar_cost;
84  }
85  double AstarCost() {
86  return astar_cost;
87  }
88 
90  return queue;
91  }
92 
93  int ID() {
94  return id;
95  }
96 
97 protected:
102 
107  virtual int create_child_nodes(dpNode**& nodes) = 0;
109  virtual double calc_cost() = 0;
111  virtual double calc_astar_cost(dpNode* potential_parent) {
112  return 0.0;
113  }
115  virtual int same_state(dpNode* refnode) = 0;
117  virtual int is_goal() = 0;
120  int open(int _max_nodes, int _max_goals);
121  void add_child(dpNode* n);
122  void remove_single();
123  void remove();
124 
125  dpNode* next_depth(dpNode* first_child = 0) {
126  dpNode* c, *ret = 0;
127  for(c = first_child ? first_child->brother : child; c && !ret; c=c->brother)
128  {
129  if(c->active) return c;
130  else if((ret = c->next_depth())) return ret;
131  }
132  if(parent) return parent->next_depth(this);
133  return 0;
134  }
135 
137  if(depth >= refnode->depth && active) return this;
138  dpNode* ret = 0;
139  if((ret = brother->next_breadth(refnode))) return ret;
140  return child->next_breadth(refnode);
141  }
142 
143  virtual void dump(ostream& ost) {
144  }
145  void dump_trajectory(ostream& ost) {
146  parent->dump_trajectory(ost);
147  dump(ost);
148  }
149  void dump_all(ostream& ost) {
150  for(int i=0; i<depth; i++) ost << " " << flush;
151  dump(ost);
152  child->dump_all(ost);
153  brother->dump_all(ost);
154  }
155 
156  double cost; // cost from parent to this node
157  double astar_cost;
158 
159  // automatically set
163  double total_cost; // total cost from start to this node
167  int id;
168  int depth;
169  int active;
170 };
171 
177 class dpQueue
178 {
179  friend class dpNode;
180  friend class dpMain;
181 public:
183  node = n;
184  parent = 0;
185  smaller_child = 0;
186  larger_child = 0;
187  active = true;
188  }
190  if(larger_child) delete larger_child;
191  if(smaller_child) delete smaller_child;
192  }
193 
194 protected:
195  void add_queue(dpQueue* q) {
196  if(q->node->total_astar_cost >= node->total_astar_cost)
197  {
198  if(!larger_child)
199  {
200  larger_child = q;
201  q->parent = this;
202  }
203  else larger_child->add_queue(q);
204  }
205  else
206  {
207  if(!smaller_child)
208  {
209  smaller_child = q;
210  q->parent = this;
211  }
212  else smaller_child->add_queue(q);
213  }
214 
215  }
217  dpQueue* ret = 0;
218 // cerr << "smallest: " << flush;
219 // node->dump(cerr);
220  if(smaller_child) ret = smaller_child->smallest();
221  if(!ret)
222  {
223  if(active)
224  {
225  ret = this;
226  }
227  else if(larger_child) ret = larger_child->smallest();
228  }
229  return ret;
230  }
232  dpQueue* ret = larger_goal_sub();
233  dpQueue* q = this;
234  while(!ret && q->parent)
235  {
236  if(q->parent->node->is_goal() &&
237  q->parent->smaller_child == q)
238  {
239  ret = q->parent;
240  }
241  else
242  {
243  if(q->parent->smaller_child == q)
244  ret = q->parent->larger_goal_sub();
245  }
246  q = q->parent;
247  }
248  return ret;
249  }
251  if(larger_child) return larger_child->smallest_goal();
252  return 0;
253  }
254 
256  dpQueue* ret = 0;
257  if(smaller_child) ret = smaller_child->smallest_goal();
258  if(!ret)
259  {
260  if(node->is_goal()) ret = this;
261  else if(larger_child) ret = larger_child->smallest_goal();
262  }
263  return ret;
264  }
265 
266  void dump(ostream& ost) {
267  ost << "--- id = " << node->id << endl;
268  ost << " cost = " << node->total_cost << ", goal = " << node->is_goal() << endl;
269  ost << " smaller = " << (smaller_child ? smaller_child->node->id : -1) << ", larger = " << (larger_child ? larger_child->node->id : -1) << endl;
270  smaller_child->dump(ost);
271  larger_child->dump(ost);
272  }
273 
278 
279  int active;
280 };
281 
286 class dpMain
287 {
288  friend class dpQueue;
289  friend class dpNode;
290 public:
291  dpMain() {
292  start_node = 0;
293  top_queue = 0;
294  n_goals = 0;
295  n_nodes = 0;
296  }
297  virtual ~dpMain() {
298  if(start_node) delete start_node;
299  if(top_queue) delete top_queue;
300  }
301 
303  void SetStartNode(dpNode* _n) {
304  start_node = _n;
305  add_node(_n);
306  if(_n->is_goal()) n_goals++;
307  }
308 
313 
319  int Search(int _max_nodes = -1, int _max_goals = -1);
321  int Search(double max_time);
323  int SearchDepthFirst(int _max_nodes = -1, int _max_goals = -1);
325  int SearchBreadthFirst(int _max_nodes = -1, int _max_goals = -1);
328  // clear all nodes and edges
329  virtual void ClearAll() {
330  reset();
331  }
332 
333  void DumpTrajectory(ostream& ost, dpNode* goal);
334 
336  dpNode* BestGoal(dpNode* ref = 0) {
337  if(!top_queue) return 0;
338  dpQueue* g = 0;
339  if(ref) g = ref->queue->larger_goal();
340  else g = top_queue->smallest_goal();
341  if(g) return g->node;
342  return 0;
343  }
344 
345  int NumNodes() {
346  return n_nodes;
347  }
348  int NumGoals() {
349  return n_goals;
350  }
351  void DumpAll(ostream& ost) {
352  cerr << "--" << endl;
353  start_node->dump_all(ost);
354  cerr << "--" << endl;
355  }
356  void DumpQueue(ostream& ost) {
357  top_queue->dump(ost);
358  }
359 
360 protected:
361  void reset() {
362  if(start_node) delete start_node;
363  if(top_queue) delete top_queue;
364  n_nodes = 0;
365  n_goals = 0;
366  }
367  void add_node(dpNode* _n) {
368  _n->dp_main = this;
369  _n->id = n_nodes;
370  n_nodes++;
371  dpQueue* q = new dpQueue(_n);
372  add_queue(q);
373  _n->queue = q;
374  }
375  void add_queue(dpQueue* _q) {
376  if(!top_queue)
377  {
378  top_queue = _q;
379  return;
380  }
381  top_queue->add_queue(_q);
382  }
384  if(!top_queue) return 0;
385  return top_queue->smallest();
386  }
387  void remove_node(dpNode* node) {
388  node->queue->active = false;
389  node->remove(); // remove all child nodes
390  }
392  node->queue->active = false;
393  node->remove_single(); // remove all child nodes
394  }
395  int is_best(dpNode* ref, dpNode* target);
396 
398  return start_node->next_breadth(refnode);
399  }
400 
403  int n_goals;
404  int n_nodes;
405 };
406 
407 
408 #endif
int id
Definition: dp.h:167
int c
Definition: autoplay.py:16
double TotalAstarCost()
Definition: dp.h:82
double total_astar_cost
Definition: dp.h:164
double total_cost
Definition: dp.h:163
dpNode * next_depth(dpNode *first_child=0)
Definition: dp.h:125
double AstarCost()
Definition: dp.h:85
void add_queue(dpQueue *q)
Definition: dp.h:195
double TotalCost()
Definition: dp.h:76
dpQueue * smallest_goal()
Definition: dp.h:255
Definition: dp.h:177
~dpQueue()
Definition: dp.h:189
void remove_node(dpNode *node)
Definition: dp.h:387
dpNode * next_breadth(dpNode *refnode)
Definition: dp.h:397
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
int NumNodes()
Definition: dp.h:345
dpQueue * larger_child
Definition: dp.h:277
dpQueue * smallest_queue()
Definition: dp.h:383
dpQueue * larger_goal_sub()
Definition: dp.h:250
virtual ~dpNode()
Definition: dp.h:47
dpNode * GetParent(int id)
Definition: dp.h:69
int NumGoals()
Definition: dp.h:348
dpQueue * Queue()
Definition: dp.h:89
png_uint_32 i
Definition: png.h:2735
int n_nodes
Definition: dp.h:404
virtual int is_goal()=0
Check if the state is goal.
dpMain()
Definition: dp.h:291
double cost
Definition: dp.h:156
dpNode * parent
Definition: dp.h:160
void SetStartNode(dpNode *_n)
Set the initial node for search.
Definition: dp.h:303
dpNode * Brother()
Definition: dp.h:55
void remove_node_single(dpNode *node)
Definition: dp.h:391
void DumpQueue(ostream &ost)
Definition: dp.h:356
int active
Definition: dp.h:279
dpNode * child
Definition: dp.h:162
void remove_single()
Definition: dp.cpp:178
dpNode * next_breadth(dpNode *refnode)
Definition: dp.h:136
void dump(ostream &ost)
Definition: dp.h:266
int n_goals
Definition: dp.h:403
dpMain * dp_main
Definition: dp.h:165
virtual void ClearAll()
Definition: dp.h:329
virtual ~dpMain()
Definition: dp.h:297
double astar_cost
Definition: dp.h:157
void dump(const SDOPackage::NVList &nv)
Definition: dp.h:286
void DumpAll(ostream &ost)
Definition: dp.h:351
dpNode()
Definition: dp.h:34
void add_queue(dpQueue *_q)
Definition: dp.h:375
id
dpQueue * smaller_child
Definition: dp.h:276
dpNode * Parent()
Definition: dp.h:52
dpQueue * queue
Definition: dp.h:166
dpNode * Child()
Definition: dp.h:58
int Depth(dpNode *target_p=0)
Definition: dp.h:62
dpQueue * larger_goal()
Definition: dp.h:231
void dump_all(ostream &ost)
Definition: dp.h:149
virtual double calc_astar_cost(dpNode *potential_parent)
Compute the A-star cost at the node (optional).
Definition: dp.h:111
dpQueue * smallest()
Definition: dp.h:216
dpNode * node
Definition: dp.h:274
dpQueue * top_queue
Definition: dp.h:401
dpQueue * parent
Definition: dp.h:275
void remove()
Definition: dp.cpp:184
dpQueue(dpNode *n)
Definition: dp.h:182
void reset()
Definition: dp.h:361
double Cost()
Definition: dp.h:79
dpNode * start_node
Definition: dp.h:402
dpNode * BestGoal(dpNode *ref=0)
Extract the goal with the smallest cost (if any).
Definition: dp.h:336
friend class dpQueue
Definition: dp.h:31
int ID()
Definition: dp.h:93
Definition: dp.h:29
void dump_trajectory(ostream &ost)
Definition: dp.h:145
void add_node(dpNode *_n)
Definition: dp.h:367
int active
Definition: dp.h:169
dpNode * brother
Definition: dp.h:161
virtual void dump(ostream &ost)
Definition: dp.h:143
int depth
Definition: dp.h:168


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:03