00001 #include "cyclic_cg_heuristic.h"
00002 #include <algorithm>
00003 #include <cassert>
00004 #include <vector>
00005 #include <set>
00006 #include "plannerParameters.h"
00007 #include "scheduler.h"
00008
00009 class CausalGraph;
00010 using namespace std;
00011
00012 LocalProblem::LocalProblem(CyclicCGHeuristic* _owner, int the_var_no,
00013 int the_start_value) :
00014 owner(_owner), base_priority(-1.0), var_no(the_var_no),
00015 causal_graph_parents(NULL), start_value(the_start_value)
00016 {
00017 }
00018
00019 double LocalTransition::get_direct_cost()
00020 {
00021 double ret = 0.0;
00022 assert(label);
00023 if(label->duration_variable != -1) {
00024 if(label->duration_variable == -2) {
00025 assert(false);
00026 ScheduledOperator *s_op = dynamic_cast<ScheduledOperator*>(label->op);
00027 assert(s_op);
00028 g_HACK()->set_waiting_time(max(g_HACK()->get_waiting_time(), s_op->time_increment));
00029 } else if(g_variable_types[label->duration_variable] == costmodule) {
00030 predicateCallbackType pct = getPreds;
00031 numericalFluentCallbackType nct = getFuncs;
00032 ret = g_cost_modules[label->duration_variable]->checkCost(
00033 g_cost_modules[label->duration_variable]->params, pct, nct, 1);
00034
00035 return ret;
00036 } else {
00037 LocalProblemNode *source = get_source();
00038 ret = source->children_state[duration_var_local];
00039 }
00040 }
00041
00042 if(ret < 0.0)
00043 ret = 0.0;
00044 return ret;
00045 }
00046
00047 inline CyclicCGHeuristic* LocalTransition::g_HACK()
00048 {
00049 return get_source()->owner->owner;
00050 }
00051
00052 void LocalTransitionDiscrete::on_source_expanded(const TimeStampedState &state)
00053 {
00054 assert(source->cost >= 0);
00055 assert(source->cost < LocalProblem::QUITE_A_LOT);
00056 assert(source->owner == target->owner);
00057 double duration = 0.0;
00058 unreached_conditions = 0;
00059 if(g_HACK()->is_running(this, state)) {
00060 target_cost = 0.0;
00061 } else {
00062 duration = get_direct_cost();
00063 target_cost = source->cost + duration;
00064 if(target->cost <= target_cost) {
00065
00066 return;
00067 }
00068 const vector<LocalAssignment> &prevail = label->precond;
00069 for(int i = 0; i < prevail.size(); i++) {
00070 int local_var = prevail[i].local_var;
00071 int prev_var_no = prevail[i].prev_dtg->var;
00072 if(g_variable_types[prev_var_no] == module) {
00073 continue;
00074 }
00075 assert(g_variable_types[prev_var_no]==logical ||
00076 g_variable_types[prev_var_no]==comparison);
00077 double current_val = source->children_state[local_var];
00078 if(!double_equals(current_val, prevail[i].value)) {
00079 int current_val_int = static_cast<int>(current_val);
00080 LocalProblem *child_problem = g_HACK()->get_local_problem(
00081 prev_var_no, current_val_int);
00082 if(!child_problem->is_initialized()) {
00083 double base_priority = source->priority();
00084 child_problem->initialize(base_priority, current_val_int, state);
00085 }
00086 int prev_value = static_cast<int> (prevail[i].value);
00087 LocalProblemNode *cond_node =
00088 child_problem->get_node(prev_value);
00089 if(!double_equals(cond_node->reached_by_wait_for, -1.0)) {
00090 assert(!g_parameters.cg_heuristic_zero_cost_waiting_transitions
00091 || cond_node->cost == 0.0);
00092 assert(cond_node->cost < LocalProblem::QUITE_A_LOT);
00093 g_HACK()->set_waiting_time(max(g_HACK()->get_waiting_time(),
00094 cond_node->reached_by_wait_for - EPS_TIME));
00095 } else if(cond_node->expanded) {
00096 target_cost = target_cost + cond_node->cost;
00097 if(target->cost <= target_cost) {
00098
00099 return;
00100 }
00101 } else {
00102 unreached_conditions++;
00103 cond_node->add_to_waiting_list(this, i);
00104 }
00105 }
00106 }
00107 }
00108 try_to_fire();
00109 }
00110
00111 void LocalTransitionDiscrete::on_condition_reached(int , double cost)
00112 {
00113 assert(unreached_conditions);
00114 --unreached_conditions;
00115 target_cost = target_cost + cost;
00116 try_to_fire();
00117 }
00118
00119 void LocalTransitionDiscrete::try_to_fire()
00120 {
00121 if(!unreached_conditions && target_cost < target->cost) {
00122 target->cost = target_cost;
00123 target->reached_by = this;
00124 target->pred = this;
00125 g_HACK()->add_to_queue(target);
00126 }
00127 }
00128
00129 void LocalTransitionDiscrete::print_description()
00130 {
00131 cout << "<" << source->owner->var_no << "|" << source->value << ","
00132 << target->value << "> (" << this << "), prevails: ";
00133 for(int i = 0; i < label->precond.size(); i++) {
00134 cout
00135 << (*source->owner->causal_graph_parents)[label->precond[i].local_var]
00136 << ": " << label->precond[i].value << " ";
00137 }
00138 }
00139
00140 LocalProblemNodeDiscrete::LocalProblemNodeDiscrete(
00141 LocalProblemDiscrete *owner_, int children_state_size, int the_value) :
00142 LocalProblemNode(owner_, children_state_size, the_value)
00143 {
00144 expanded = false;
00145 reached_by_wait_for = -1.0;
00146 reached_by = 0;
00147 pred = 0;
00148 }
00149
00150 void LocalProblemNodeDiscrete::on_expand(const TimeStampedState &state)
00151 {
00152 expanded = true;
00153
00154 if(reached_by) {
00155 LocalProblemNode *parent = reached_by->get_source();
00156 children_state = parent->children_state;
00157 const vector<LocalAssignment> &prevail = reached_by->label->precond;
00158 for(int i = 0; i < prevail.size(); i++) {
00159 children_state[prevail[i].local_var] = prevail[i].value;
00160 }
00161 const vector<LocalAssignment> &cyclic_effects =
00162 reached_by->label->effect;
00163 for(int i = 0; i < cyclic_effects.size(); i++) {
00164 if(g_variable_types[cyclic_effects[i].prev_dtg->var] == logical) {
00165 children_state[cyclic_effects[i].local_var]
00166 = cyclic_effects[i].value;
00167 } else {
00168 assert(g_variable_types[cyclic_effects[i].prev_dtg->var] == primitive_functional);
00169 const LocalAssignment &la = cyclic_effects[i];
00170 updatePrimitiveNumericVariable(la.fop, la.local_var, la.var,
00171 children_state);
00172 }
00173 }
00174 if(parent->reached_by)
00175 reached_by = parent->reached_by;
00176 }
00177 for(const_it_waiting_list it = waiting_list.begin(); it
00178 != waiting_list.end(); ++it) {
00179 it->first->on_condition_reached(it->second, cost);
00180 }
00181 waiting_list.clear();
00182
00183 for(int i = 0; i < additional_outgoing_transitions.size(); i++) {
00184 additional_outgoing_transitions[i].on_source_expanded(state);
00185 }
00186
00187 for(int i = 0; i < outgoing_transitions.size(); i++) {
00188 outgoing_transitions[i].on_source_expanded(state);
00189 }
00190
00191 return;
00192 }
00193
00194
00195 bool LocalProblemNode::all_conds_satiesfied(const ValueTransitionLabel *label, const TimeStampedState &state)
00196 {
00197 for(int i = 0; i < label->precond.size(); ++i) {
00198 int var = label->precond[i].prev_dtg->var;
00199 if(g_variable_types[var] != module &&
00200 !double_equals(label->precond[i].value, state[var])) {
00201 return false;
00202 }
00203 }
00204 return true;
00205 }
00206
00207 void LocalProblemNode::mark_helpful_transitions(const TimeStampedState &state)
00208 {
00209 if(!this->owner->is_initialized()) {
00210
00211 return;
00212 }
00213 assert(cost >= 0 && cost < LocalProblem::QUITE_A_LOT);
00214 if(reached_by) {
00215 double duration = 0.0;
00216 int duration_variable = reached_by->label->duration_variable;
00217 if(reached_by->label->duration_variable == -2) {
00218 ScheduledOperator *s_op =
00219 dynamic_cast<ScheduledOperator*> (reached_by->label->op);
00220 assert(s_op);
00221 duration = s_op->time_increment;
00222 } else if(!(duration_variable == -1)) {
00223 if(g_variable_types[duration_variable] == costmodule) {
00224 predicateCallbackType pct = getPreds;
00225 numericalFluentCallbackType nct = getFuncs;
00226 duration = g_cost_modules[duration_variable]->checkCost(
00227 g_cost_modules[duration_variable]->params, pct, nct, 1);
00228
00229 } else {
00230 duration = reached_by->get_source()->children_state[reached_by->duration_var_local];
00231 }
00232 }
00233 if(double_equals(reached_by->target_cost, duration)) {
00234 assert(reached_by->label);
00235
00236
00237
00238 if(reached_by->label->op && all_conds_satiesfied(reached_by->label, state)) {
00239 const Operator *op = reached_by->label->op;
00240 g_HACK()->set_preferred(op);
00241 }
00242 } else {
00243
00244 if(!g_HACK()->is_running(reached_by, state)) {
00245 const vector<LocalAssignment> &prevail = reached_by->label->precond;
00246 for(int i = 0; i < prevail.size(); i++) {
00247 int prev_var_no = prevail[i].prev_dtg->var;
00248
00249 if(g_variable_types[prev_var_no] == module)
00250 continue;
00251
00252 int prev_value = static_cast<int> (prevail[i].value);
00253 int value = static_cast<int> (state[prev_var_no]);
00254
00255 if(prev_value == value)
00256 continue;
00257
00258 LocalProblemNode *child_node = g_HACK()->get_local_problem(
00259 prev_var_no, value)->get_node(prev_value);
00260 assert(child_node);
00261 assert(child_node->cost < LocalProblem::QUITE_A_LOT);
00262 if(child_node->cost < LocalProblem::QUITE_A_LOT &&
00263 double_equals(child_node->reached_by_wait_for, -1.0)) {
00264 child_node->mark_helpful_transitions(state);
00265 }
00266 }
00267 }
00268 }
00269 }
00270 }
00271
00272 void LocalProblemNodeDiscrete::dump()
00273 {
00274 cout << "---------------" << endl;
00275 print_name();
00276 cout << "Waiting list:" << endl;
00277 for(const_it_waiting_list it = waiting_list.begin(); it
00278 != waiting_list.end(); ++it) {
00279 cout << " ";
00280 it->first->print_description();
00281 cout << "," << it->second << endl;
00282 }
00283 cout << "Context:" << endl;
00284 if(!expanded)
00285 cout << " not set yet!" << endl;
00286 else {
00287 for(int i = 0; i < owner->causal_graph_parents->size(); i++) {
00288 cout << (*owner->causal_graph_parents)[i] << ": ";
00289 cout << children_state[i] << endl;
00290 }
00291 }
00292 cout << "cost: " << cost << endl;
00293 cout << "priority: " << priority() << endl;
00294 cout << "reached_by: ";
00295 if(reached_by && reached_by->label && reached_by->label->op) {
00296 cout << reached_by->label->op->get_name() << endl;
00297 } else {
00298 cout << "axiom" << endl;
00299 }
00300 cout << "pred: ";
00301 if(pred && pred->label && pred->label->op) {
00302 cout << pred->label->op->get_name() << endl;
00303 } else {
00304 cout << "axiom" << endl;
00305 }
00306 cout << "---------------" << endl;
00307 }
00308
00309 void LocalProblemNodeDiscrete::print_name()
00310 {
00311 cout << "Local Problem [" << owner->var_no << ","
00312 << (dynamic_cast<LocalProblemDiscrete*> (owner))->start_value
00313 << "], node " << value << " (" << this << ")" << endl;
00314 }
00315
00316 void LocalProblemDiscrete::compile_DTG_arcs_to_LTD_objects(
00317 DomainTransitionGraphSymb *dtgs)
00318 {
00319 for(int value = 0; value < nodes.size(); value++) {
00320 LocalProblemNodeDiscrete &node = nodes[value];
00321 node.outgoing_transitions.clear();
00322 node.additional_outgoing_transitions.clear();
00323 ValueNode &dtg_node = dtgs->nodes[value];
00324 for(int i = 0; i < dtg_node.transitions.size(); i++) {
00325 ValueTransition &dtg_trans = dtg_node.transitions[i];
00326 int target_value = dtg_trans.target->value;
00327 LocalProblemNodeDiscrete &target = nodes[target_value];
00328 for(int j = 0; j < dtg_trans.ccg_labels.size(); j++) {
00329 ValueTransitionLabel *label = dtg_trans.ccg_labels[j];
00330 LocalTransitionDiscrete trans(label, &node, &target);
00331 trans.duration_var_local = getLocalIndexOfGlobalVariable(
00332 label->duration_variable);
00333 assert(trans.duration_var_local != -1 || label->duration_variable == -1 || label->duration_variable == -2);
00334 node.outgoing_transitions.push_back(trans);
00335 }
00336 }
00337 for(int i = 0; i < dtg_node.additional_transitions.size(); i++) {
00338 ValueTransition &dtg_trans = dtg_node.additional_transitions[i];
00339 int target_value = dtg_trans.target->value;
00340 LocalProblemNodeDiscrete &target = nodes[target_value];
00341 for(int j = 0; j < dtg_trans.ccg_labels.size(); j++) {
00342 ValueTransitionLabel *label = dtg_trans.ccg_labels[j];
00343 LocalTransitionDiscrete trans(label, &node, &target);
00344 trans.duration_var_local = getLocalIndexOfGlobalVariable(
00345 label->duration_variable);
00346 assert(trans.duration_var_local != -1 || label->duration_variable == -1 || label->duration_variable == -2);
00347 node.additional_outgoing_transitions.push_back(trans);
00348 }
00349 }
00350 }
00351 }
00352
00353 void LocalProblemDiscrete::build_nodes_for_variable(int var_no)
00354 {
00355 if(!(g_variable_types[var_no] == logical)) {
00356 cout << "variable type: " << g_variable_types[var_no] << endl;
00357 assert(false);
00358 }
00359 DomainTransitionGraph *dtg = g_transition_graphs[var_no];
00360 DomainTransitionGraphSymb *dtgs =
00361 dynamic_cast<DomainTransitionGraphSymb *> (dtg);
00362 assert(dtgs);
00363 causal_graph_parents = &dtg->ccg_parents;
00364 global_to_local_parents = &dtg->global_to_local_ccg_parents;
00365 int num_parents = causal_graph_parents->size();
00366 for(int value = 0; value < g_variable_domain[var_no]; value++)
00367 nodes.push_back(LocalProblemNodeDiscrete(this, num_parents, value));
00368 compile_DTG_arcs_to_LTD_objects(dtgs);
00369 }
00370
00371 void LocalProblemDiscrete::build_nodes_for_goal()
00372 {
00373
00374
00375 causal_graph_parents = new vector<int> ;
00376 for(int i = 0; i < g_goal.size(); i++)
00377 causal_graph_parents->push_back(g_goal[i].first);
00378
00379 for(int value = 0; value < 2; value++)
00380 nodes.push_back(LocalProblemNodeDiscrete(this, g_goal.size(), value));
00381
00382 vector<LocalAssignment> goals;
00383 for(int i = 0; i < g_goal.size(); i++) {
00384 int goal_var = g_goal[i].first;
00385 double goal_value = g_goal[i].second;
00386 DomainTransitionGraph *goal_dtg = g_transition_graphs[goal_var];
00387 goals.push_back(LocalAssignment(goal_dtg, i, goal_value, end_cond));
00388 }
00389 ValueTransitionLabel *label = new ValueTransitionLabel(-1, goals, end);
00390 LocalProblemNodeDiscrete *target = &nodes[1];
00391 LocalProblemNodeDiscrete *start = &nodes[0];
00392 assert(label);
00393 LocalTransitionDiscrete trans(label, start, target);
00394
00395 nodes[0].outgoing_transitions.push_back(trans);
00396 }
00397
00398 LocalProblemDiscrete::LocalProblemDiscrete(CyclicCGHeuristic* _owner, int the_var_no, int the_start_val) :
00399 LocalProblem(_owner, the_var_no, the_start_val)
00400 {
00401 if(var_no == -1)
00402 build_nodes_for_goal();
00403 else
00404 build_nodes_for_variable(var_no);
00405 int parents_num = causal_graph_parents->size();
00406 nodes[0].children_state.resize(parents_num, 0.0);
00407 nodes[1].children_state.resize(parents_num, 0.0);
00408
00409 if(var_no != -1) {
00410 depending_vars.resize(parents_num);
00411 children_in_cg.resize(parents_num);
00412 buildDependingVars(parents_num);
00413 }
00414 }
00415
00416 void LocalProblemDiscrete::initialize(double base_priority_, int start_value,
00417 const TimeStampedState &state)
00418 {
00419 assert(!is_initialized());
00420 base_priority = base_priority_;
00421
00422 for(int to_value = 0; to_value < nodes.size(); to_value++) {
00423 nodes[to_value].expanded = false;
00424 nodes[to_value].cost = QUITE_A_LOT;
00425 nodes[to_value].reached_by = NULL;
00426 nodes[to_value].pred = NULL;
00427 nodes[to_value].waiting_list.clear();
00428 nodes[to_value].reached_by_wait_for = -1.0;
00429 }
00430 LocalProblemNodeDiscrete *start = &nodes[start_value];
00431 start->cost = 0;
00432 int parents_num = causal_graph_parents->size();
00433 for(int i = 0; i < parents_num; i++) {
00434 int var = (*causal_graph_parents)[i];
00435 start->children_state[i] = state[var];
00436 }
00437 owner->add_to_queue(start);
00438 if(g_parameters.cg_heuristic_fire_waiting_transitions_only_if_local_problems_matches_state) {
00439 if(!(double_equals(state[var_no], start_value))) {
00440 return;
00441 }
00442 }
00443 for(int i = 0; i < state.scheduled_effects.size(); i++) {
00444 const ScheduledEffect &seffect = state.scheduled_effects[i];
00445 if(seffect.var == var_no) {
00446 LocalProblemNodeDiscrete& node = nodes[static_cast<int>(seffect.post)];
00447 if(g_parameters.cg_heuristic_zero_cost_waiting_transitions)
00448 node.cost = 0.0;
00449 else
00450 node.cost = seffect.time_increment;
00451 assert(seffect.time_increment > 0);
00452 assert(seffect.time_increment < LocalProblem::QUITE_A_LOT);
00453 node.reached_by_wait_for = seffect.time_increment;
00454 for(int i = 0; i < parents_num; ++i) {
00455 int var = (*causal_graph_parents)[i];
00456 node.children_state[i] = state[var];
00457 }
00458 node.on_expand(state);
00459 }
00460 }
00461 }
00462
00463 vector<TimedOp> LocalProblem::generate_causal_constraints(LocalProblemNode* goalNode, set<
00464 CausalConstraint>& constraints, vector<TimedOp>& neededOps,
00465 const TimeStampedState& state, set<const Operator*>& labels)
00466 {
00467 LocalProblemNode* startNode = get_node(start_value);
00468 vector<LocalTransition*> path;
00469 LocalProblemNode* helpNode = goalNode;
00470 while(helpNode != startNode && helpNode->pred && !owner->is_running(
00471 helpNode->pred, state)) {
00472 path.push_back(helpNode->pred);
00473 helpNode = helpNode->pred->get_source();
00474 }
00475 bool transIsOp = false;
00476 int indexOfMainTrans = -1;
00477 int indexOfLastMainTrans = -1;
00478 vector<TimedOp> returnOps = vector<TimedOp> ();
00479 for(int i = 0; i < path.size(); ++i) {
00480 LocalTransition* trans = path[i];
00481 if(!(owner->is_running(trans, state))) {
00482 if(trans->label->op) {
00483 Operator *op = trans->label->op;
00484 if(trans->label && (labels.find(op) != labels.end())) {
00485
00486 }
00487 double duration = trans->get_direct_cost();
00488 indexOfMainTrans = neededOps.size();
00489 TimedOp newOp = tr1::make_tuple(op, duration, indexOfMainTrans);
00490 neededOps.push_back(newOp);
00491 labels.insert(op);
00492 transIsOp = true;
00493 for(int l = 0; l < returnOps.size(); ++l) {
00494 if(tr1::get<0>(neededOps[indexOfMainTrans])->isDisabledBy(
00495 tr1::get<0>(returnOps[l]))) {
00496
00497 constraints.insert(make_pair(indexOfMainTrans,
00498 tr1::get<2>(returnOps[l])));
00499 }
00500 }
00501 } else {
00502 indexOfMainTrans = -1;
00503 transIsOp = false;
00504 }
00505 const vector<LocalAssignment>& preconds = trans->label->precond;
00506 vector<vector<TimedOp> > subplans;
00507 for(int j = 0; j < preconds.size(); ++j) {
00508 vector<TimedOp> newOps = vector<TimedOp> ();
00509 double actual_value =
00510 trans->get_source()->children_state[preconds[j].local_var];
00511 if(!(double_equals(actual_value, preconds[j].value))) {
00512 int var = preconds[j].prev_dtg->var;
00513 LocalProblem* sub_problem = owner->get_local_problem(var,
00514 static_cast<int> (actual_value));
00515 newOps = sub_problem->generate_causal_constraints(
00516 (sub_problem->get_node(
00517 static_cast<int> (preconds[j].value))),
00518 constraints, neededOps, state, labels);
00519 if(transIsOp) {
00520
00521 for(int k = 0; k < newOps.size(); ++k) {
00522
00523
00524 constraints.insert(make_pair(indexOfMainTrans + 1
00525 + k, indexOfMainTrans));
00526 for(int l = 0; l < returnOps.size(); ++l) {
00527
00528 if(tr1::get<0>(newOps[k])->isDisabledBy(
00529 tr1::get<0>(returnOps[l]))) {
00530
00531
00532 constraints.insert(make_pair(tr1::get<2>(
00533 newOps[k]), tr1::get<2>(
00534 returnOps[l])));
00535 } else {
00536
00537 }
00538 }
00539 }
00540
00541 }
00542 }
00543 subplans.push_back(newOps);
00544 }
00545 assert(subplans.size() == preconds.size());
00546
00547 if(transIsOp) {
00548 returnOps.push_back(tr1::make_tuple(trans->label->op,
00549 trans->get_direct_cost(), indexOfMainTrans));
00550 }
00551 for(int k = 0; k < subplans.size(); ++k) {
00552 returnOps.insert(returnOps.end(), subplans[k].begin(),
00553 subplans[k].end());
00554 }
00555
00556
00557
00558
00559
00560 for(int k = 0; k < subplans.size() - 1; ++k) {
00561 if(subplans.size() == 0)
00562 break;
00563 for(int l = 0; l < subplans[k].size(); ++l) {
00564 TimedOp& firstAction = subplans[k][l];
00565 for(int s = k + 1; s < subplans.size(); ++s) {
00566 for(int m = 0; m < subplans[s].size(); ++m) {
00567 TimedOp& secondAction = subplans[s][m];
00568 if(tr1::get<0>(firstAction)->isDisabledBy(
00569 tr1::get<0>(secondAction))) {
00570
00571 constraints.insert(
00572 make_pair(tr1::get<2>(firstAction),
00573 tr1::get<2>(secondAction)));
00574 }
00575 }
00576 }
00577 }
00578 }
00579 if(transIsOp && indexOfLastMainTrans != -1) {
00580
00581 }
00582 indexOfLastMainTrans = indexOfMainTrans;
00583 } else {
00584 cout << "Error: Owner was running" << endl;
00585 }
00586 }
00587 return returnOps;
00588 }
00589
00590 void LocalTransitionComp::print_description()
00591 {
00592 const FuncTransitionLabel *label_func =
00593 dynamic_cast<const FuncTransitionLabel*> (label);
00594 assert(label_func);
00595 cout << "<" << source->owner->var_no << "|" << source->value << "> ("
00596 << this << "), prevails: ";
00597 for(int i = 0; i < label->precond.size(); i++) {
00598 cout
00599 << (*source->owner->causal_graph_parents)[label->precond[i].local_var]
00600 << ": " << label->precond[i].value << " ";
00601 }
00602 cout << "; affecting variable: " << label_func->starting_variable << " ";
00603 }
00604
00605 LocalProblemNodeComp::LocalProblemNodeComp(LocalProblemComp *owner_,
00606 int children_state_size, int the_value, binary_op the_op) :
00607 LocalProblemNode(owner_, children_state_size, the_value), op(the_op)
00608 {
00609 expanded = false;
00610 opened = false;
00611 reached_by_wait_for = -1.0;
00612 reached_by = NULL;
00613 pred = NULL;
00614 bestTransition = NULL;
00615 }
00616
00617 void LocalProblemNode::add_to_waiting_list(LocalTransition *transition,
00618 int prevail_no)
00619 {
00620 waiting_list.push_front(make_pair(transition, prevail_no));
00621 }
00622
00623 inline CyclicCGHeuristic* LocalProblemNode::g_HACK()
00624 {
00625 return owner->owner;
00626 }
00627
00628 void LocalProblemNodeComp::expand(LocalTransitionComp* trans)
00629 {
00630 LocalProblemComp *prob = dynamic_cast<LocalProblemComp*> (owner);
00631 LocalProblemNodeComp *target_node = &(prob->nodes[1 - prob->start_value]);
00632 target_node->reached_by = trans;
00633 target_node->pred = trans;
00634 target_node->expanded = true;
00635 expanded = true;
00636
00637
00638 for(const_it_waiting_list it = target_node->waiting_list.begin(); it
00639 != target_node->waiting_list.end(); ++it) {
00640 it->first->on_condition_reached(it->second, target_node->cost);
00641 }
00642 target_node->waiting_list.clear();
00643 }
00644
00645 void LocalProblemNodeComp::fire(LocalTransitionComp* trans)
00646 {
00647
00648 trans->target_cost += trans->get_direct_cost();
00649 if(trans->target_cost < trans->target->cost) {
00650 trans->target->cost = trans->target_cost;
00651 trans->target->bestTransition = trans;
00652 bestTransition = trans;
00653 trans->target->opened = true;
00654 }
00655 }
00656
00657 void LocalProblemNodeComp::on_expand(const TimeStampedState &state)
00658 {
00659 if(opened) {
00660 if(bestTransition) {
00661 bestTransition->source->expand(bestTransition);
00662 }
00663 return;
00664 }
00665 opened = true;
00666
00667 vector<LocalTransitionComp*> ready_transitions;
00668 nodes_where_this_subscribe.resize(outgoing_transitions.size());
00669 vector<double> temp_children_state(children_state.size());
00670 for(int i = 0; i < outgoing_transitions.size(); i++) {
00671 LocalTransitionComp *trans = &outgoing_transitions[i];
00672 temp_children_state = children_state;
00673 updateNumericVariables((*trans), temp_children_state);
00674 if(check_progress_of_transition(temp_children_state, trans)) {
00675 if(is_satiesfied(i, trans, state)) {
00676 ready_transitions.push_back(trans);
00677 } else if(g_HACK()->is_running(trans, state)) {
00678 ready_transitions.push_back(trans);
00679 }
00680 }
00681 }
00682 if(!ready_transitions.empty()) {
00683 for(int i = 0; i < ready_transitions.size(); ++i) {
00684 fire(ready_transitions[i]);
00685 }
00686 assert(bestTransition);
00687 assert(bestTransition->target);
00688 g_HACK()->add_to_queue(bestTransition->target);
00689 }
00690 subscribe_to_waiting_lists();
00691 return;
00692 }
00693
00694 bool LocalProblemNodeComp::check_progress_of_transition(
00695 vector<double> &temp_children_state, LocalTransitionComp *trans)
00696 {
00697 double &old_value = children_state[0];
00698 double &new_value = temp_children_state[0];
00699 LocalProblemNodeComp *node = dynamic_cast<LocalProblemNodeComp*>(trans->get_source());
00700 assert(node);
00701 binary_op comp_op = node->op;
00702 switch (comp_op) {
00703 case lt:
00704 case le:
00705 return new_value < old_value;
00706 case gt:
00707 case ge:
00708 return new_value > old_value;
00709 case eq:
00710 return abs(new_value) < abs(old_value);
00711 case ue:
00712 return !(double_equals(new_value, 0));
00713 default:
00714 return false;
00715 }
00716 }
00717
00718 void LocalTransitionComp::on_condition_reached(int cond_no, double cost)
00719 {
00720 if(!source->opened || source->expanded || conds_satiesfied[cond_no] == true) {
00721 return;
00722 }
00723 target_cost += cost;
00724 assert(conds_satiesfied[cond_no] == false);
00725 conds_satiesfied[cond_no] = true;
00726 for(int i = 0; i < conds_satiesfied.size(); i++)
00727 if(conds_satiesfied[i] == false)
00728 return;
00729
00730 target_cost += get_direct_cost();
00731 target->cost = target_cost;
00732 target->bestTransition = this;
00733 source->bestTransition = this;
00734 target->opened = true;
00735 g_HACK()->add_to_queue(target);
00736 }
00737
00738 bool CyclicCGHeuristic::is_running(LocalTransition* trans, const TimeStampedState& state)
00739 {
00740 if(!trans->label || !trans->label->op)
00741 return false;
00742 for(int i = 0; i < state.operators.size(); ++i) {
00743 if(!(state.operators[i].get_name().compare(trans->label->op->get_name()))) {
00744 set_waiting_time(max(get_waiting_time(), state.operators[i].time_increment - EPS_TIME));
00745 return true;
00746 }
00747 }
00748 return false;
00749 }
00750
00751 bool LocalProblemNodeComp::is_satiesfied(int trans_index,
00752 LocalTransitionComp* trans, const TimeStampedState &state)
00753 {
00754 bool ret = true;
00755 for(int i = 0; i < trans->label->precond.size(); i++) {
00756 const LocalAssignment &pre_cond = trans->label->precond[i];
00757
00758 int global_var = (*(owner->causal_graph_parents))[pre_cond.local_var];
00759 if(g_variable_types[global_var] == module)
00760 continue;
00761 assert(!is_functional(global_var));
00762 double current_val = children_state[pre_cond.local_var];
00763 if(double_equals(current_val, pre_cond.value)) {
00764
00765 assert(trans->conds_satiesfied[i] == false);
00766 trans->conds_satiesfied[i] = true;
00767 continue;
00768 }
00769 int current_val_int = static_cast<int>(current_val);
00770 LocalProblem *child_problem = g_HACK()->get_local_problem(global_var,
00771 current_val_int);
00772 if(!child_problem->is_initialized()) {
00773 double base_priority = priority();
00774 child_problem->initialize(base_priority, current_val_int, state);
00775 }
00776 int prev_value = static_cast<int> (pre_cond.value);
00777 LocalProblemNode *cond_node = child_problem->get_node(prev_value);
00778 if(!double_equals(cond_node->reached_by_wait_for, -1.0)) {
00779 g_HACK()->set_waiting_time(max(g_HACK()->get_waiting_time(),
00780 cond_node->reached_by_wait_for - EPS_TIME));
00781 assert(trans->target_cost < LocalProblem::QUITE_A_LOT);
00782 assert(trans->conds_satiesfied[i] == false);
00783 trans->conds_satiesfied[i] = true;
00784 } else if(cond_node->expanded) {
00785 trans->target_cost = trans->target_cost + cond_node->cost;
00786 assert(trans->conds_satiesfied[i] == false);
00787 trans->conds_satiesfied[i] = true;
00788 } else {
00789 nodes_where_this_subscribe[trans_index].push_back(make_pair(
00790 cond_node, i));
00791
00792 ret = false;
00793 }
00794 }
00795 return ret;
00796 }
00797
00798 bool LocalProblemNodeComp::is_directly_satiesfied(
00799 const LocalAssignment &pre_cond)
00800 {
00801 if(double_equals(children_state[pre_cond.local_var], pre_cond.value))
00802 return true;
00803 return false;
00804 }
00805
00806 void LocalProblemNodeComp::subscribe_to_waiting_lists()
00807 {
00808 for(int i = 0; i < nodes_where_this_subscribe.size(); i++) {
00809 for(int j = 0; j < nodes_where_this_subscribe[i].size(); j++) {
00810 LocalProblemNode *prob = nodes_where_this_subscribe[i][j].first;
00811 int prevail_no = nodes_where_this_subscribe[i][j].second;
00812 prob->add_to_waiting_list(&(outgoing_transitions[i]), prevail_no);
00813 }
00814 }
00815 }
00816
00817 void LocalProblemNodeComp::updateNumericVariables(LocalTransitionComp &trans,
00818 vector<double> &temp_children_state)
00819 {
00820 const FuncTransitionLabel* label_func =
00821 dynamic_cast<const FuncTransitionLabel*> (trans.label);
00822 assert(label_func);
00823 int primitive_var_local = label_func->starting_variable;
00824 assert(g_variable_types[(*owner->causal_graph_parents)[primitive_var_local]]
00825 == primitive_functional);
00826 int influencing_var_local = label_func->influencing_variable;
00827 assert(is_functional((*owner->causal_graph_parents)[influencing_var_local]));
00828 assignment_op a_op = label_func->a_op;
00829 updatePrimitiveNumericVariable(a_op, primitive_var_local,
00830 influencing_var_local, temp_children_state);
00831 }
00832
00833 void LocalProblemNode::updatePrimitiveNumericVariable(assignment_op a_op,
00834 int primitive_var_local, int influencing_var_local,
00835 vector<double> &temp_children_state)
00836 {
00837 double &new_value = temp_children_state[primitive_var_local];
00838 double &influencing_value = temp_children_state[influencing_var_local];
00839 switch (a_op) {
00840 case assign:
00841 new_value = influencing_value;
00842 break;
00843 case scale_up:
00844 new_value *= influencing_value;
00845 break;
00846 case scale_down:
00847 if(double_equals(influencing_value, 0.0)) {
00848 if(new_value < 0)
00849 new_value = REALLYBIG;
00850 else
00851 new_value = REALLYSMALL;
00852 } else
00853 new_value /= influencing_value;
00854 break;
00855 case increase:
00856 new_value += influencing_value;
00857 break;
00858 case decrease:
00859 new_value -= influencing_value;
00860 break;
00861 default:
00862 assert(false);
00863 break;
00864 }
00865 updateNumericVariablesRec(primitive_var_local, temp_children_state);
00866 }
00867
00868 void LocalProblemNode::updateNumericVariablesRec(int local_var,
00869 vector<double> &temp_children_state)
00870 {
00871 for(int i = 0; i < owner->depending_vars[local_var].size(); i++) {
00872 int var_to_update = owner->depending_vars[local_var][i];
00873 if(!(owner->children_in_cg[var_to_update].size() == 3)) {
00874
00875
00876 continue;
00877 }
00878 binary_op
00879 bop =
00880 static_cast<binary_op> (owner->children_in_cg[var_to_update][2]);
00881 int left_var = owner->children_in_cg[var_to_update][0];
00882 int right_var = owner->children_in_cg[var_to_update][1];
00883 if(g_variable_types[(*(owner->causal_graph_parents))[var_to_update]]
00884 == subterm_functional) {
00885 updateSubtermNumericVariables(var_to_update, bop, left_var,
00886 right_var, temp_children_state);
00887 } else {
00888 assert(g_variable_types[(*(owner->causal_graph_parents))[var_to_update]] == comparison);
00889 updateComparisonVariables(var_to_update, bop, left_var, right_var,
00890 temp_children_state);
00891 }
00892 }
00893 }
00894
00895 int LocalProblem::getLocalIndexOfGlobalVariable(int global_var)
00896 {
00897 hashmap::const_iterator iter;
00898 iter = global_to_local_parents->find(global_var);
00899 if(iter == global_to_local_parents->end())
00900 return -1;
00901 return (*iter).second;
00902 }
00903
00904 void LocalProblemNode::updateComparisonVariables(int var, binary_op op,
00905 int left_var, int right_var, vector<double> &temp_children_state)
00906 {
00907 double &left = temp_children_state[left_var];
00908 double &right = temp_children_state[right_var];
00909 double &target = temp_children_state[var];
00910 switch (op) {
00911 case lt:
00912 if(left + EPSILON < right) {
00913 target = 0;
00914 } else {
00915 target = 1;
00916 }
00917 break;
00918 case le:
00919 if(left + EPSILON < right || double_equals(left, right)) {
00920 target = 0;
00921 } else {
00922 target = 1;
00923 }
00924 break;
00925 case eq:
00926 if(double_equals(left, right)) {
00927 target = 0;
00928 } else {
00929 target = 1;
00930 }
00931 break;
00932 case gt:
00933 if(left + EPSILON > right) {
00934 target = 0;
00935 } else {
00936 target = 1;
00937 }
00938 break;
00939 case ge:
00940 if(left + EPSILON > right || !double_equals(left, right)) {
00941 target = 0;
00942 } else {
00943 target = 1;
00944 }
00945 break;
00946 case ue:
00947 if(!double_equals(left, right)) {
00948 target = 0;
00949 } else {
00950 target = 1;
00951 }
00952
00953 default:
00954 assert(false);
00955 break;
00956 }
00957 updateNumericVariablesRec(var, temp_children_state);
00958 }
00959
00960 void LocalProblemNode::updateSubtermNumericVariables(int var, binary_op op,
00961 int left_var, int right_var, vector<double> &temp_children_state)
00962 {
00963 double &left = temp_children_state[left_var];
00964 double &right = temp_children_state[right_var];
00965 double &target = temp_children_state[var];
00966 switch (op) {
00967 case add:
00968 target = left + right;
00969 break;
00970 case subtract:
00971 target = left - right;
00972 break;
00973 case mult:
00974 target = left * right;
00975 break;
00976 case divis:
00977 if(double_equals(right, 0.0)) {
00978 if(left < 0)
00979 target = REALLYBIG;
00980 else
00981 target = REALLYSMALL;
00982 } else
00983 target = left / right;
00984 break;
00985 default:
00986 assert(false);
00987 break;
00988 }
00989 updateNumericVariablesRec(var, temp_children_state);
00990 }
00991
00992 void LocalProblemNodeComp::dump()
00993 {
00994 cout << "---------------" << endl;
00995 print_name();
00996 cout << "Waiting list:" << endl;
00997 for(const_it_waiting_list it = waiting_list.begin(); it
00998 != waiting_list.end(); ++it) {
00999 cout << " ";
01000 it->first->print_description();
01001 cout << "," << it->second << endl;
01002 }
01003 cout << "Context:" << endl;
01004 if(!opened)
01005 cout << " not set yet!" << endl;
01006 else {
01007 for(int i = 0; i < owner->causal_graph_parents->size(); i++) {
01008 cout << (*owner->causal_graph_parents)[i] << ": ";
01009 cout << children_state[i] << endl;
01010 }
01011 }
01012 cout << "cost: " << cost << endl;
01013 cout << "priority: " << priority() << endl;
01014 cout << "---------------" << endl;
01015 }
01016
01017 void LocalProblemNodeComp::print_name()
01018 {
01019 cout << "Local Problem [" << owner->var_no << ","
01020 << (dynamic_cast<LocalProblemComp*> (owner))->start_value
01021 << "], node " << value << " (" << this << ")" << endl;
01022 }
01023
01024 void LocalProblem::buildDependingVars(int parents_num)
01025 {
01026 for(int i = 0; i < parents_num; i++) {
01027 int context_variable = (*causal_graph_parents)[i];
01028 const vector<int>& current_depending_vars =
01029 g_causal_graph->get_successors(context_variable);
01030 for(int j = 0; j < current_depending_vars.size(); j++) {
01031 int current_depending_var = current_depending_vars[j];
01032 if(g_variable_types[current_depending_var] == comparison
01033 || g_variable_types[current_depending_var]
01034 == subterm_functional) {
01035 int idx = getLocalIndexOfGlobalVariable(current_depending_var);
01036 if(idx != -1)
01037 depending_vars[i].push_back(idx);
01038 }
01039 }
01040 if(g_variable_types[context_variable] == subterm_functional) {
01041 DomainTransitionGraph *dtg = g_transition_graphs[context_variable];
01042 DomainTransitionGraphSubterm *dtgs =
01043 dynamic_cast<DomainTransitionGraphSubterm*> (dtg);
01044 assert(dtgs);
01045 int left_var = getLocalIndexOfGlobalVariable(dtgs->left_var);
01046 int right_var = getLocalIndexOfGlobalVariable(dtgs->right_var);
01047 if(left_var != -1 && right_var != -1) {
01048 assert(children_in_cg[i].size() == 0);
01049 children_in_cg[i].push_back(left_var);
01050 children_in_cg[i].push_back(right_var);
01051 children_in_cg[i].push_back(dtgs->op);
01052 }
01053 } else if(g_variable_types[context_variable] == comparison) {
01054 DomainTransitionGraph *dtg = g_transition_graphs[context_variable];
01055 DomainTransitionGraphComp *dtgc =
01056 dynamic_cast<DomainTransitionGraphComp*> (dtg);
01057 assert(dtgc);
01058 int left_var = getLocalIndexOfGlobalVariable(
01059 dtgc->nodes.first.left_var);
01060 int right_var = getLocalIndexOfGlobalVariable(
01061 dtgc->nodes.first.right_var);
01062 if(left_var != -1 && right_var != -1) {
01063 assert(children_in_cg[i].size() == 0);
01064 children_in_cg[i].push_back(left_var);
01065 children_in_cg[i].push_back(right_var);
01066 children_in_cg[i].push_back(dtgc->nodes.first.op);
01067 }
01068 }
01069 }
01070 }
01071
01072 LocalProblemComp::LocalProblemComp(CyclicCGHeuristic* _owner, int the_var_no, int the_start_value) :
01073 LocalProblem(_owner, the_var_no, the_start_value)
01074 {
01075 assert(var_no >= 0);
01076 build_nodes_for_variable(var_no, the_start_value);
01077
01078 int parents_num = causal_graph_parents->size();
01079 nodes[start_value].children_state.resize(parents_num, 0.0);
01080
01081 depending_vars.resize(parents_num);
01082 children_in_cg.resize(parents_num);
01083 buildDependingVars(parents_num);
01084 }
01085
01086 void LocalProblemComp::build_nodes_for_variable(int var_no, int the_start_value)
01087 {
01088 if(!(g_variable_types[var_no] == comparison)) {
01089 cout << "variable type: " << g_variable_types[var_no] << endl;
01090 assert(false);
01091 }
01092 DomainTransitionGraph *dtg = g_transition_graphs[var_no];
01093 DomainTransitionGraphComp *dtgc =
01094 dynamic_cast<DomainTransitionGraphComp *> (dtg);
01095 assert(dtgc);
01096
01097 causal_graph_parents = &dtgc->ccg_parents;
01098 global_to_local_parents = &dtgc->global_to_local_ccg_parents;
01099
01100 int num_parents = causal_graph_parents->size();
01101
01102 assert(g_variable_domain[var_no] == 3);
01103
01104
01105 nodes.push_back(LocalProblemNodeComp(this, num_parents, 0, dtgc->nodes.second.op));
01106 nodes.push_back(LocalProblemNodeComp(this, num_parents, 1, dtgc->nodes.first.op));
01107
01108
01109 for(int i = 0; i < dtgc->transitions.size(); i++) {
01110 LocalTransitionComp trans(&(dtgc->transitions[i]),
01111 &(nodes[the_start_value]), &(nodes[the_start_value - 1]));
01112
01113 trans.duration_var_local = dtgc->transitions[i].duration_variable;
01114 nodes[the_start_value].outgoing_transitions.push_back(trans);
01115 }
01116 }
01117
01118 void LocalProblemComp::initialize(double base_priority_, int start_value,
01119 const TimeStampedState &state)
01120 {
01121 assert(start_value == this->start_value);
01122
01123 int target_value = abs(start_value - 1);
01124
01125 assert(!is_initialized());
01126 base_priority = base_priority_;
01127
01128 assert(nodes.size() == 2);
01129
01130 for(int to_value = 0; to_value < nodes.size(); to_value++) {
01131 nodes[to_value].expanded = false;
01132 nodes[to_value].opened = false;
01133 nodes[to_value].reached_by = NULL;
01134 nodes[to_value].pred = NULL;
01135 nodes[to_value].waiting_list.clear();
01136 nodes[to_value].nodes_where_this_subscribe.clear();
01137 nodes[to_value].bestTransition = NULL;
01138 nodes[to_value].reached_by_wait_for = -1.0;
01139 }
01140
01141 nodes[start_value].cost = 0;
01142 nodes[target_value].cost = LocalProblem::QUITE_A_LOT;
01143
01144 for(int i = 0; i < nodes[start_value].outgoing_transitions.size(); i++) {
01145 LocalTransitionComp &trans = nodes[start_value].outgoing_transitions[i];
01146 trans.target_cost = 0;
01147 for(int j = 0; j < trans.label->precond.size(); j++) {
01148 trans.conds_satiesfied[j] = false;
01149 }
01150 }
01151
01152 int parents_num = causal_graph_parents->size();
01153 for(int i = 0; i < parents_num; i++) {
01154 int var = (*causal_graph_parents)[i];
01155 nodes[start_value].children_state[i] = state[var];
01156 }
01157 owner->add_to_queue(&nodes[start_value]);
01158 }
01159
01160 CyclicCGHeuristic::CyclicCGHeuristic(Mode _mode) : mode(_mode)
01161 {
01162 goal_problem = 0;
01163 goal_node = 0;
01164 }
01165
01166 CyclicCGHeuristic::~CyclicCGHeuristic()
01167 {
01168 delete goal_problem;
01169 for(int i = 0; i < local_problems.size(); i++)
01170 delete local_problems[i];
01171 }
01172
01173 void CyclicCGHeuristic::initialize()
01174 {
01175 assert(goal_problem == 0);
01176 cout << "Initializing cyclic causal graph heuristic...";
01177
01178 int num_variables = g_variable_domain.size();
01179
01180 goal_problem = new LocalProblemDiscrete(this, -1, 0);
01181 goal_node = &goal_problem->nodes[1];
01182
01183 local_problem_index.resize(num_variables);
01184 for(int var_no = 0; var_no < num_variables; var_no++) {
01185 int num_values = g_variable_domain[var_no];
01186 if(num_values == -1 || num_values == -2 || num_values == -3) {
01187
01188 assert(g_variable_types[var_no] == subterm_functional || g_variable_types[var_no] == primitive_functional
01189 || g_variable_types[var_no] == module || g_variable_types[var_no] == costmodule);
01190 } else {
01191 local_problem_index[var_no].resize(num_values, NULL);
01192 }
01193 }
01194 cout << "done." << endl;
01195 }
01196
01197 double CyclicCGHeuristic::compute_heuristic(const TimeStampedState &state)
01198 {
01199 if(state.satisfies(g_goal) && state.operators.empty())
01200 return 0.0;
01201
01202
01203
01204 g_setModuleCallbackState(&state);
01205
01206 initialize_queue();
01207 set_waiting_time(REALLYSMALL);
01208 goal_problem->base_priority = -1;
01209 for(int i = 0; i < local_problems.size(); i++)
01210 local_problems[i]->base_priority = -1;
01211
01212 goal_problem->initialize(0.0, 0, state);
01213
01214 double heuristic = compute_costs(state);
01215 double scheduledPlanMakespan = 0.0;
01216
01217 if(heuristic != DEAD_END && heuristic != 0) {
01218 goal_node->mark_helpful_transitions(state);
01219 }
01220 if(heuristic != DEAD_END && !(mode == COST || mode == CEA)) {
01221 scheduledPlanMakespan = computeScheduledPlanMakespan(state);
01222 }
01223
01224 if(heuristic == DEAD_END)
01225 return DEAD_END;
01226
01227 if(mode == COST) {
01228 cout << "mode = COST" << endl;
01229 return heuristic;
01230 } else if(mode == REMAINING_MAKESPAN) {
01231 cout << "mode = RM" << endl;
01232 return scheduledPlanMakespan;
01233 } else if(mode == SUFFIX_MAKESPAN) {
01234 double longestRunningAction = 0.0;
01235 for(int i = 0; i < state.operators.size(); ++i) {
01236 if(state.operators[i].time_increment > longestRunningAction) {
01237 longestRunningAction = state.operators[i].time_increment;
01238 }
01239 }
01240
01241 assert(longestRunningAction <= scheduledPlanMakespan);
01242 scheduledPlanMakespan -= longestRunningAction;
01243 if(!state.operators.empty() && (double_equals(scheduledPlanMakespan, 0.0))) {
01244 scheduledPlanMakespan = 0.9;
01245 }
01246 return scheduledPlanMakespan;
01247 } else if(mode == WEIGHTED) {
01248 assert(heuristic != DEAD_END);
01249 if(get_waiting_time() - EPSILON > 0.0) {
01250 heuristic += get_waiting_time();
01251 }
01252 double longestRunningAction = 0.0;
01253 for(int i = 0; i < state.operators.size(); ++i) {
01254 if(state.operators[i].time_increment > longestRunningAction) {
01255 longestRunningAction = state.operators[i].time_increment;
01256 }
01257 }
01258 assert(longestRunningAction <= scheduledPlanMakespan);
01259 scheduledPlanMakespan -= longestRunningAction;
01260 return scheduledPlanMakespan + 0.2 * heuristic;
01261 }
01262
01263 assert(mode == CEA);
01264 assert(heuristic != DEAD_END);
01265 if(!state.operators.empty() && (heuristic == 0.0)) {
01266 heuristic += 0.9;
01267 }
01268 if(get_waiting_time() - EPSILON > 0.0) {
01269 heuristic += get_waiting_time();
01270 }
01271 return heuristic;
01272 }
01273
01274 double CyclicCGHeuristic::computeScheduledPlanMakespan(const TimeStampedState & state) const
01275 {
01276 set<CausalConstraint> constraints;
01277 vector<TimedOp> needed_ops;
01278 for(int i = 0; i < state.operators.size(); ++i) {
01279 needed_ops.push_back(tr1::make_tuple(&state.operators[i],
01280 state.operators[i].time_increment, i));
01281 }
01282 set<const Operator*> labels;
01283 goal_problem->generate_causal_constraints(goal_node, constraints, needed_ops,
01284 state, labels);
01285
01286
01287 for(int i = 0; i < state.operators.size(); ++i) {
01288 for(int j = state.operators.size(); j < needed_ops.size(); ++j) {
01289 if(tr1::get<0>(needed_ops[i])->isDisabledBy(tr1::get<0>(
01290 needed_ops[j])) || tr1::get<0>(needed_ops[i])->enables(
01291 tr1::get<0>(needed_ops[j]))) {
01292 constraints.insert(make_pair(i, j));
01293 }
01294 }
01295 }
01296
01297
01298 vector<string> variable_names;
01299 for(int i = 0; i < needed_ops.size(); ++i) {
01300 variable_names.push_back(tr1::get<0>(needed_ops[i])->get_name()
01301 + "s");
01302 }
01303 for(int i = 0; i < needed_ops.size(); ++i) {
01304 variable_names.push_back(tr1::get<0>(needed_ops[i])->get_name()
01305 + "e");
01306 }
01307 SimpleTemporalProblem stn(variable_names);
01308
01309
01310 for(int i = 0; i < needed_ops.size(); ++i) {
01311 stn.setUnboundedIntervalFromXZero(i, 0.0);
01312
01313
01314 stn.setSingletonInterval(i, i + needed_ops.size(), tr1::get<1>(
01315 needed_ops[i]));
01316 }
01317
01318
01319 for(set<CausalConstraint>::iterator it = constraints.begin(); it
01320 != constraints.end(); ++it) {
01321 stn.setUnboundedInterval(needed_ops.size() + it->first, it->second,
01322 EPSILON);
01323 }
01324
01325 stn.setCurrentDistancesAsDefault();
01326 bool isValid = stn.solveWithP3C();
01327 assert(isValid);
01328 vector<Happening> happenings = stn.getHappenings(true);
01329 if(!stn.solution_is_valid()) {
01330 cout << "Ops:" << endl;
01331 for(int i = 0; i < needed_ops.size(); ++i) {
01332 cout << i << ": " << tr1::get<0>(needed_ops[i])->get_name()
01333 << ", duration: " << tr1::get<1>(needed_ops[i]) << endl;
01334 }
01335 cout << "Constraints:" << endl;
01336 set<CausalConstraint>::iterator it;
01337 for(it = constraints.begin(); it != constraints.end(); ++it) {
01338 cout << it->first << " <<< " << it->second << endl;
01339 }
01340 }
01341
01342 assert(stn.solution_is_valid());
01343
01344 double heuristicNew = stn.getMaximalTimePointInTightestSchedule(true);
01345 assert(heuristicNew >= 0.0);
01346 return heuristicNew;
01347 }
01348
01349 void CyclicCGHeuristic::initialize_queue()
01350 {
01351 open_nodes = node_queue();
01352 }
01353
01354 void CyclicCGHeuristic::add_to_queue(LocalProblemNode *node)
01355 {
01356 open_nodes.push(node);
01357 }
01358
01359 LocalProblemNode* CyclicCGHeuristic::remove_from_queue()
01360 {
01361 LocalProblemNode* ret = open_nodes.top();
01362 open_nodes.pop();
01363 return ret;
01364 }
01365
01366 double CyclicCGHeuristic::compute_costs(const TimeStampedState &state)
01367 {
01368 while(!open_nodes.empty()) {
01369 LocalProblemNode *node = remove_from_queue();
01370 if(node == goal_node)
01371 return node->cost;
01372 if(!(node->expanded)) {
01373 node->on_expand(state);
01374 }
01375 }
01376 return DEAD_END;
01377 }