dp.cpp
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  */
14 #include "dp.h"
15 //#include <dims_timer.h>
16 
17 int dpMain::Search(int _max_nodes, int _max_goals)
18 {
19  if(!start_node)
20  {
21  cerr << "dpMain error: start node not set" << endl;
22  return -1;
23  }
24  dpQueue* q = top_queue;
25  while(q)
26  {
27  if(q->node->open(_max_nodes, _max_goals)) break;
29  q = smallest_queue();
30  }
31  return 0;
32 }
33 
34 #if 0
35 int dpMain::Search(double max_time)
36 {
37  if(!start_node)
38  {
39  cerr << "dpMain error: start node not set" << endl;
40  return -1;
41  }
42  dpQueue* q = top_queue;
43  LongInteger start_tick = GetTick();
44  while(q)
45  {
46  if(q->node->open(-1, -1)) break;
48  q = smallest_queue();
49  LongInteger cur_tick = GetTick();
50  if(ExpiredTime(start_tick, cur_tick) > max_time) return 1;
51  }
52  return 0;
53 }
54 #endif
55 
56 int dpMain::SearchDepthFirst(int _max_nodes, int _max_goals)
57 {
58  if(!start_node)
59  {
60  cerr << "dpMain error: start node not set" << endl;
61  return -1;
62  }
63  dpNode* node = start_node;
64  while(node)
65  {
66  DumpAll(cerr);
67  if(node->open(_max_nodes, _max_goals)) break;
68  remove_node_single(node);
69  node = node->next_depth();
70  }
71  return 0;
72 }
73 
74 int dpMain::SearchBreadthFirst(int _max_nodes, int _max_goals)
75 {
76  if(!start_node)
77  {
78  cerr << "dpMain error: start node not set" << endl;
79  return -1;
80  }
81  dpNode* node = start_node;
82  while(node)
83  {
84  if(node->open(_max_nodes, _max_goals)) break;
85  remove_node_single(node);
86  node = next_breadth(node);
87  DumpAll(cerr);
88  }
89  return 0;
90 }
91 
92 int dpNode::open(int _max_nodes, int _max_goals)
93 {
94  int i, n_nodes = 0;
95  dpNode** nodes = 0;
96  n_nodes = create_child_nodes(nodes);
97  if(n_nodes == 0) return 0;
98  if(!nodes) return 0;
99  for(i=0; i<n_nodes; i++)
100  {
101  dpNode* n = nodes[i];
102  if(!n) continue;
103  n->brother = child; // added by nishimura
104  n->parent = this; // added by nishimura
105  n->depth = depth+1; // added by nishimura
106  n->cost = n->calc_cost();
107  n->astar_cost = n->calc_astar_cost(this);
108  n->total_cost = total_cost + n->cost;
110  if(!dp_main->is_best(dp_main->start_node, n)) // check if there is better node
111  {
112  n->brother = 0;
113  delete n;
114  nodes[i] = 0;
115  continue;
116  }
117 // add_child(n); // removed by nishimura
118  child = n; // added by nishimura
119  dp_main->add_node(n); // added by nishimura
120 
121  if(n->is_goal()) dp_main->n_goals++;
122  }
123  delete[] nodes;
124  if(_max_nodes >= 0 && dp_main->n_nodes >= _max_nodes){
125  return -1;
126  }
127  if(_max_goals >= 0 && dp_main->n_goals >= _max_goals){
128  return -1;
129  }
130  return 0;
131 }
132 
133 int dpMain::is_best(dpNode* ref, dpNode* target)
134 {
135  if(!ref) return true;
136  int ret = true;
137  if(ref != target && target->same_state(ref))
138  {
139 // if(target->total_astar_cost <= ref->total_astar_cost)
140  if(target->total_cost <= ref->total_cost)
141  {
142  // target is better: remove ref
143  remove_node(ref);
144  }
145  else
146  {
147  // ref is better: remove target
148  ret = false;
149  }
150  }
151  else
152  {
153  dpNode* n;
154  int myret = true;
155  for(n=ref->child; n && myret; n=n->brother)
156  {
157  myret = is_best(n, target);
158  }
159  ret = myret;
160  }
161  return ret;
162 }
163 
164 void dpMain::DumpTrajectory(ostream& ost, dpNode* goal)
165 {
166  goal->dump_trajectory(ost);
167 }
168 
170 {
171  n->brother = child;
172  child = n;
173  n->parent = this;
174  n->depth = depth+1;
175  dp_main->add_node(n);
176 }
177 
179 {
180  active = false;
181  queue->active = false;
182 }
183 
185 {
186  dpNode* n;
187  for(n=child; n; n=n->brother) n->remove();
188  active = false;
189  queue->active = false;
190 }
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
void DumpTrajectory(ostream &ost, dpNode *goal)
Definition: dp.cpp:164
Definition: dp.h:177
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 SearchBreadthFirst(int _max_nodes=-1, int _max_goals=-1)
Breadth-first search.
Definition: dp.cpp:74
int Search(int _max_nodes=-1, int _max_goals=-1)
Dijkstra or A* search: find the node with the smallest cost.
Definition: dp.cpp:17
dpQueue * smallest_queue()
Definition: dp.h:383
Base class for generic discrete search.
virtual int same_state(dpNode *refnode)=0
Check if the node&#39;s state is the same as refnode. (to remove duplicates)
int is_best(dpNode *ref, dpNode *target)
Definition: dp.cpp:133
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.
double cost
Definition: dp.h:156
int open(int _max_nodes, int _max_goals)
Definition: dp.cpp:92
dpNode * parent
Definition: dp.h:160
void remove_node_single(dpNode *node)
Definition: dp.h:391
void add_child(dpNode *n)
Definition: dp.cpp:169
dpNode * child
Definition: dp.h:162
void remove_single()
Definition: dp.cpp:178
double astar_cost
Definition: dp.h:157
void DumpAll(ostream &ost)
Definition: dp.h:351
virtual double calc_astar_cost(dpNode *potential_parent)
Compute the A-star cost at the node (optional).
Definition: dp.h:111
dpNode * node
Definition: dp.h:274
virtual double calc_cost()=0
Compute the local cost at the node.
dpQueue * top_queue
Definition: dp.h:401
void remove()
Definition: dp.cpp:184
dpNode * start_node
Definition: dp.h:402
Definition: dp.h:29
int SearchDepthFirst(int _max_nodes=-1, int _max_goals=-1)
Depth-first search.
Definition: dp.cpp:56
void dump_trajectory(ostream &ost)
Definition: dp.h:145
dpNode * brother
Definition: dp.h:161
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