dp.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00014 #include "dp.h"
00015 //#include <dims_timer.h>
00016 
00017 int dpMain::Search(int _max_nodes, int _max_goals)
00018 {
00019         if(!start_node)
00020         {
00021                 cerr << "dpMain error: start node not set" << endl;
00022                 return -1;
00023         }
00024         dpQueue* q = top_queue;
00025         while(q)
00026         {
00027                 if(q->node->open(_max_nodes, _max_goals)) break;
00028                 remove_node_single(q->node);
00029                 q = smallest_queue();
00030         }
00031         return 0;
00032 }
00033 
00034 #if 0
00035 int dpMain::Search(double max_time)
00036 {
00037         if(!start_node)
00038         {
00039                 cerr << "dpMain error: start node not set" << endl;
00040                 return -1;
00041         }
00042         dpQueue* q = top_queue;
00043         LongInteger start_tick = GetTick();
00044         while(q)
00045         {
00046                 if(q->node->open(-1, -1)) break;
00047                 remove_node_single(q->node);
00048                 q = smallest_queue();
00049                 LongInteger cur_tick = GetTick();
00050                 if(ExpiredTime(start_tick, cur_tick) > max_time) return 1;
00051         }
00052         return 0;
00053 }
00054 #endif
00055 
00056 int dpMain::SearchDepthFirst(int _max_nodes, int _max_goals)
00057 {
00058         if(!start_node)
00059         {
00060                 cerr << "dpMain error: start node not set" << endl;
00061                 return -1;
00062         }
00063         dpNode* node = start_node;
00064         while(node)
00065         {
00066                 DumpAll(cerr);
00067                 if(node->open(_max_nodes, _max_goals)) break;
00068                 remove_node_single(node);
00069                 node = node->next_depth();
00070         }
00071         return 0;
00072 }
00073 
00074 int dpMain::SearchBreadthFirst(int _max_nodes, int _max_goals)
00075 {
00076         if(!start_node)
00077         {
00078                 cerr << "dpMain error: start node not set" << endl;
00079                 return -1;
00080         }
00081         dpNode* node = start_node;
00082         while(node)
00083         {
00084                 if(node->open(_max_nodes, _max_goals)) break;
00085                 remove_node_single(node);
00086                 node = next_breadth(node);
00087                 DumpAll(cerr);
00088         }
00089         return 0;
00090 }
00091 
00092 int dpNode::open(int _max_nodes, int _max_goals)
00093 {
00094         int i, n_nodes = 0;
00095         dpNode** nodes = 0;
00096         n_nodes = create_child_nodes(nodes);
00097         if(n_nodes == 0) return 0;
00098         if(!nodes) return 0;
00099         for(i=0; i<n_nodes; i++)
00100         {
00101                 dpNode* n = nodes[i];
00102                 if(!n) continue;
00103                 n->brother = child; // added by nishimura
00104                 n->parent = this; // added by nishimura
00105                 n->depth = depth+1; // added by nishimura
00106                 n->cost = n->calc_cost();
00107                 n->astar_cost = n->calc_astar_cost(this);
00108                 n->total_cost = total_cost + n->cost;
00109                 n->total_astar_cost = n->total_cost + n->astar_cost;
00110                 if(!dp_main->is_best(dp_main->start_node, n))  // check if there is better node
00111                 {
00112                         n->brother = 0;
00113                         delete n;
00114                         nodes[i] = 0;
00115                         continue;
00116                 }
00117 //              add_child(n); // removed by nishimura
00118                 child = n; // added by nishimura
00119                 dp_main->add_node(n); // added by nishimura
00120                 
00121                 if(n->is_goal()) dp_main->n_goals++;
00122         }
00123         delete[] nodes;
00124         if(_max_nodes >= 0 && dp_main->n_nodes >= _max_nodes){
00125                 return -1;
00126         }
00127         if(_max_goals >= 0 && dp_main->n_goals >= _max_goals){
00128                 return -1;
00129         }
00130         return 0;
00131 }
00132 
00133 int dpMain::is_best(dpNode* ref, dpNode* target)
00134 {
00135         if(!ref) return true;
00136         int ret = true;
00137         if(ref != target && target->same_state(ref))
00138         {
00139 //              if(target->total_astar_cost <= ref->total_astar_cost)
00140                 if(target->total_cost <= ref->total_cost)
00141                 {
00142                         // target is better: remove ref
00143                         remove_node(ref);
00144                 }
00145                 else
00146                 {
00147                         // ref is better: remove target
00148                         ret = false;
00149                 }
00150         }
00151         else
00152         {
00153                 dpNode* n;
00154                 int myret = true;
00155                 for(n=ref->child; n && myret; n=n->brother)
00156                 {
00157                         myret = is_best(n, target);
00158                 }
00159                 ret = myret;
00160         }
00161         return ret;
00162 }
00163 
00164 void dpMain::DumpTrajectory(ostream& ost, dpNode* goal)
00165 {
00166         goal->dump_trajectory(ost);
00167 }
00168 
00169 void dpNode::add_child(dpNode* n)
00170 {
00171         n->brother = child;
00172         child = n;
00173         n->parent = this;
00174         n->depth = depth+1;
00175         dp_main->add_node(n);
00176 }
00177 
00178 void dpNode::remove_single()
00179 {
00180         active = false;
00181         queue->active = false;
00182 }
00183 
00184 void dpNode::remove()
00185 {
00186         dpNode* n;
00187         for(n=child; n; n=n->brother) n->remove();
00188         active = false;
00189         queue->active = false;
00190 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:16