cyclic_cg_heuristic.cpp
Go to the documentation of this file.
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             //printf("Duration from module: %f\n", ret);
00035             return ret;
00036         } else {
00037             LocalProblemNode *source = get_source();
00038             ret = source->children_state[duration_var_local];
00039         }
00040     }
00041     // HACK for modeltrain, where it appears, that the duration of a transition is negative...WHY???
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             // Transition cannot find a shorter path to target.
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);  // If zero cost is on, this should be 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                         // Transition cannot find a shorter path to target.
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 /*cond_no*/, 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     // Set children state unless this was an initial node.
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 //waiting edges are for free....check if all conditions are satiesfied indeed!
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         //condition was already satiesfied!
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                 //printf("Duration from module: %f\n", duration);
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             // if reached_by->label->op is NULL this means that the transition corresponds to
00236             // an axiom. Do not add this axiom (or rather a NULL pointer) to the set of
00237             // preferred operators.
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             // Recursively compute helpful transitions for prevailed variables.
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     // TODO: We have a small memory leak here. Could be fixed by
00374     // making two LocalProblem classes with a virtual destructor.
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                     //                    continue;
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                         //zwischen kante und allen kanten und deren cond-kanten, die im gleichen graph danach kommen
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                         //                        cout << "TRANS: " << trans->label->op->get_name() << endl;
00521                         for(int k = 0; k < newOps.size(); ++k) {
00522                             //    cout << " NEWOP: " << tr1::get<0>(newOps[k])->get_name() << endl;
00523                             //zwischen kante und allen aktionen der conditions
00524                             constraints.insert(make_pair(indexOfMainTrans + 1
00525                                         + k, indexOfMainTrans));
00526                             for(int l = 0; l < returnOps.size(); ++l) {
00527                                 //    cout << "  RETOP: " << tr1::get<0>(returnOps[l])->get_name() << endl;
00528                                 if(tr1::get<0>(newOps[k])->isDisabledBy(
00529                                             tr1::get<0>(returnOps[l]))) {
00530                                     //    cout << "   yes: " << tr1::get<2>(newOps[k]) << " << " << tr1::get<2>(returnOps[l]) << endl;
00531                                     //zwischen cond-kanten der einen und cond-kanten aller nachfolgenden
00532                                     constraints.insert(make_pair(tr1::get<2>(
00533                                                     newOps[k]), tr1::get<2>(
00534                                                         returnOps[l])));
00535                                 } else {
00536                                     //cout << "   no" << endl;
00537                                 }
00538                             }
00539                         }
00540                         //                        cout << "." << endl;
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             //            cout << "subplan sizes:" << endl;
00556             //            for(int v = 0; v < subplans.size(); ++v) {
00557             //                cout << v << ":" << subplans[v].size() << endl;
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                                 //zwischen allen aktionen der teilpläne
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                 //                constraints.insert(make_pair(indexOfMainTrans, indexOfLastMainTrans));
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     //call transitions on the own waiting lists
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     // we have to add the costs of the transition itself!
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     // we have to add the costs of the transition itself!
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         // check whether the cost of this prevail has already been computed
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             // to change nothing costs nothing
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             //the cost of this prevail has not been determined so far...
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             //      cout << "owner->children_in_cg[var_to_update].size(): " << owner->children_in_cg[var_to_update].size() << endl;
00875             //      assert(false);
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     // There are 3 values for a comp variable: false, true and undefined. In the heuristic we only have
01104     // to deal with the first both of them.
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     // Compile the DTG arcs into LocalTransition objects.
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         //variables in func_transitions of comp nodes are local!!!!
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             //we don't need local problems for functional variables so far....
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     // set the module callback state as the state we compute the heuristik for
01203     // this is probably the best we can do
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     // constraints for running actions
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     // build and solve STN...
01298     vector<string> variable_names;//(needed_ops.size()*2);
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     // assert that start time point of actions are non-negative
01310     for(int i = 0; i < needed_ops.size(); ++i) {
01311         stn.setUnboundedIntervalFromXZero(i, 0.0);
01312         // assert that differences between start and end time points are exactly
01313         // the durations of the actions
01314         stn.setSingletonInterval(i, i + needed_ops.size(), tr1::get<1>(
01315                     needed_ops[i]));
01316     }
01317 
01318     // assert that causal relationships are preserved
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 }


tfd_modules
Author(s): Maintained by Christian Dornhege (see AUTHORS file).
autogenerated on Mon Oct 6 2014 07:52:06