state.cpp
Go to the documentation of this file.
00001 #include "state.h"
00002 
00003 #include "axioms.h"
00004 #include "globals.h"
00005 #include "operator.h"
00006 #include "causal_graph.h"
00007 #include "plannerParameters.h"
00008 
00009 #include <algorithm>
00010 #include <iostream>
00011 #include <cassert>
00012 using namespace std;
00013 
00014 TimeStampedState::TimeStampedState(istream &in)
00015 {
00016     check_magic(in, "begin_state");
00017     for(int i = 0; i < g_variable_domain.size(); i++) {
00018         double var;
00019         cin >> var;
00020         state.push_back(var);
00021     }
00022     check_magic(in, "end_state");
00023 
00024     g_default_axiom_values = state;
00025     timestamp = 0.0; // + EPS_TIME;
00026 
00027     initialize();
00028 }
00029 
00030 void TimeStampedState::apply_module_effect(string internal_name)
00031 {
00032     int index = atoi(internal_name.substr(3).c_str());
00033     g_setModuleCallbackState(this);
00034     predicateCallbackType pct = getPreds;
00035     numericalFluentCallbackType nct = getFuncs;
00036 
00037     EffectModule *module = g_effect_modules[index];
00038     vector<double> new_values = vector<double> (module->writtenVars.size());
00039     ParameterList &params = module->params;
00040     module->applyEffect(params, pct, nct, new_values);
00041     for(int i = 0; i < new_values.size(); ++i) {
00042         state[module->writtenVars[i]] = new_values[i];
00043     }
00044 }
00045 
00046 TimeStampedState::TimeStampedState(const TimeStampedState &predecessor,
00047     const Operator &op) :
00048         state(predecessor.state),
00049         scheduled_effects(predecessor.scheduled_effects),
00050         conds_over_all(predecessor.conds_over_all),
00051         conds_at_end(predecessor.conds_at_end),
00052         operators(predecessor.operators)
00053 {
00054     // FIXME: The effects between now and now + sep can 
00055     // happen at different timestamps. So, to implement this method 
00056     // correctly, we have to apply and check the effects in the correct 
00057     // order (and also check effect conditions in the intermediate steps).
00058     // This is analogous to the problem in let_time_pass.
00059 
00060     double sep = (g_parameters.epsilonize_internally ? EPS_TIME : 0.0);
00061 
00062     timestamp = predecessor.timestamp + sep;
00063 
00064     // compute duration
00065     double duration = op.get_duration(&predecessor);
00066 
00067     // The scheduled effects of the new state are precisely the
00068     // scheduled effects of the predecessor state plus those at-end
00069     // effects of the given operator whose at-start conditions are
00070     // satisfied.
00071     for(int i = 0; i < op.get_pre_post_end().size(); i++) {
00072         const PrePost &eff = op.get_pre_post_end()[i];
00073         if(eff.does_fire(predecessor)) {
00074             scheduled_effects.push_back(ScheduledEffect(duration, eff));
00075         }
00076     }
00077 
00078     // The scheduled module effects of the new state are precisely the
00079     // scheduled module effects of the predecessor state plus those at-end
00080     // module effects of the given operator whose at-start conditions are
00081     // satisfied.
00082     for(int i = 0; i < op.get_mod_effs_end().size(); i++) {
00083         const ModuleEffect &mod_eff = op.get_mod_effs_end()[i];
00084         if(mod_eff.does_fire(predecessor)) {
00085             scheduled_module_effects.push_back(ScheduledModuleEffect(duration,
00086                         mod_eff));
00087         }
00088     }
00089 
00090     // Update values affected by an at-start effect of the operator.
00091     for(int i = 0; i < op.get_pre_post_start().size(); i++) {
00092         const PrePost &pre_post = op.get_pre_post_start()[i];
00093 
00094         // at-start effects may not have any at-end conditions
00095         assert(pre_post.cond_end.size() == 0);
00096 
00097         if(pre_post.does_fire(predecessor)) {
00098             apply_effect(pre_post.var, pre_post.fop, pre_post.var_post,
00099                     pre_post.post);
00100         }
00101     }
00102 
00103     // Update start module effects
00104     for(int i = 0; i < op.get_mod_effs_start().size(); ++i) {
00105         const ModuleEffect &mod_eff = op.get_mod_effs_start()[i];
00106         assert(mod_eff.cond_end.size() == 0);
00107         if(mod_eff.does_fire(predecessor)) {
00108             apply_module_effect(mod_eff.module->internal_name);
00109         }
00110     }
00111 
00112     g_axiom_evaluator->evaluate(*this);
00113 
00114     // The values of the new state are obtained by applying all
00115     // effects scheduled in the predecessor state until the new time
00116     // stamp and subsequently applying axioms
00117     for(int i = 0; i < scheduled_effects.size(); i++) {
00118         ScheduledEffect &eff = scheduled_effects[i];
00119         if((eff.time_increment + EPSILON < sep) &&
00120                          satisfies(eff.cond_end)) {
00121             apply_effect(eff.var, eff.fop, eff.var_post, eff.post);
00122         }
00123         if(eff.time_increment + EPSILON < sep) {
00124             scheduled_effects.erase(scheduled_effects.begin() + i);
00125             i--;
00126         } else {
00127             eff.time_increment -= sep;
00128         }
00129     }
00130 
00131     // same for module effects
00132     for(int i = 0; i < scheduled_module_effects.size(); i++) {
00133         ScheduledModuleEffect &eff = scheduled_module_effects[i];
00134         if((eff.time_increment + EPSILON < sep) &&
00135                          satisfies(eff.cond_end)) {
00136             apply_module_effect(eff.module->internal_name);
00137         }
00138         if(eff.time_increment + EPSILON < sep) {
00139             scheduled_module_effects.erase(scheduled_module_effects.begin() + i);
00140             i--;
00141         } else {
00142             eff.time_increment -= sep;
00143         }
00144     }
00145 
00146     g_axiom_evaluator->evaluate(*this);
00147 
00148 
00149     // The persistent over-all conditions of the new state are
00150     // precisely the persistent over-all conditions of the predecessor
00151     // state plus the over-all conditions of the newly added operator
00152     for(int i = 0; i < op.get_prevail_overall().size(); i++) {
00153         conds_over_all.push_back(ScheduledCondition(duration,
00154                     op.get_prevail_overall()[i]));
00155     }
00156 
00157     // The persistent at-end conditions of the new state are
00158     // precisely the persistent at-end conditions of the predecessor
00159     // state plus the at-end conditions of the newly added operator
00160     for(int i = 0; i < op.get_prevail_end().size(); i++) {
00161         conds_at_end.push_back(ScheduledCondition(duration, op.get_prevail_end()[i]));
00162     }
00163 
00164     // The running operators of the new state are precisely
00165     // the running operators of the predecessor state plus the newly
00166     // added operator
00167     operators.push_back(ScheduledOperator(duration, op));
00168 
00169     // timestamp += EPS_TIME;
00170     // FIXME: time increments aller Komponenten des Zustands anpassen
00171     // assert(!double_equals(timestamp, next_happening()));
00172 
00173     initialize();
00174 }
00175 
00176 double TimeStampedState::eps_time(double offset) const {
00177     // FIXME: this could be implemented much more efficiently if operators were
00178     // sorted by time_increment
00179     bool recheck = true;
00180     double time = EPS_TIME + offset;
00181     while(recheck) {
00182         recheck = false;
00183         for(unsigned int i = 0; i < operators.size(); ++i) {
00184             double increment = operators[i].time_increment;
00185             if(double_equals(increment, time)) {
00186                 time += EPS_TIME;
00187                 recheck = true;
00188                 break;
00189             }
00190         }
00191     }
00192     return time - offset;
00193 }
00194 
00195 
00196 TimeStampedState TimeStampedState::let_time_pass(
00197     bool go_to_intermediate_between_now_and_next_happening,
00198     bool skip_eps_steps) const {
00199 
00200         // FIXME: If we do not go to the intermediate between now and the 
00201     // next happening but epsilonize internally, the effects can 
00202     // happen at different timestamps. So, to implement this method 
00203     // correctly, we have to apply and check the effects in the correct 
00204     // order (and also check effect conditions in the intermediate steps).
00205 
00206     // Copy this state
00207     TimeStampedState succ(*this);
00208 
00209     // The time stamp of the new state is the minimum of the end
00210     // time points of scheduled effects in the predecessor state
00211     // and the end time points associated with persistent at-end
00212     // conditions of the predecessor state. If the flag "go to
00213     // intermediate between now and next happening" is set, it is
00214     // the intermediate time point between new and the next happening
00215     // (this is needed to safely test all persistent over-all
00216     // conditions -- otherwise we might fail to ever test some of them).
00217     double nh = next_happening();
00218     if(double_equals(nh, timestamp) ||
00219         !go_to_intermediate_between_now_and_next_happening) {
00220         succ.timestamp = nh;
00221     } else {
00222         succ.timestamp = timestamp + 0.5 * (nh - timestamp);
00223     }
00224 
00225     double time_diff = succ.timestamp - timestamp;
00226 
00227     if(skip_eps_steps && g_parameters.epsilonize_internally && 
00228        !go_to_intermediate_between_now_and_next_happening) {
00229         double additional_time_diff = eps_time(nh - timestamp);
00230         time_diff += additional_time_diff;
00231         succ.timestamp += additional_time_diff;
00232     }
00233 
00234     if(!go_to_intermediate_between_now_and_next_happening) {
00235         // The values of the new state are obtained by applying all
00236         // effects scheduled in the predecessor state for the new time
00237         // stamp and subsequently applying axioms
00238         for(int i = 0; i < scheduled_effects.size(); i++) {
00239             const ScheduledEffect &eff = scheduled_effects[i];
00240             if((eff.time_increment < time_diff + EPSILON) &&
00241                 succ.satisfies(eff.cond_end)) {
00242                 succ.apply_effect(eff.var, eff.fop, eff.var_post, eff.post);
00243             }
00244         }
00245 
00246         // Apply module effects as well!
00247         for(int i = 0; i < scheduled_module_effects.size(); i++) {
00248             const ScheduledModuleEffect &mod_eff = scheduled_module_effects[i];
00249             if((mod_eff.time_increment < time_diff + EPSILON) &&
00250                     succ.satisfies(mod_eff.cond_end)) {
00251                 succ.apply_module_effect(mod_eff.module->internal_name);
00252             }
00253         }
00254 
00255         g_axiom_evaluator->evaluate(succ);
00256     }
00257 
00258     // The scheduled effects of the new state are precisely the
00259     // scheduled effects of the predecessor state minus those
00260     // whose scheduled time point has been reached and minus those
00261     // whose over-all condition is violated.
00262     for(int i = 0; i < succ.scheduled_effects.size(); i++) {
00263         succ.scheduled_effects[i].time_increment -= time_diff;
00264     }
00265     if(!go_to_intermediate_between_now_and_next_happening) {
00266         for(int i = 0; i < succ.scheduled_effects.size(); i++) {
00267             const ScheduledEffect &eff = succ.scheduled_effects[i];
00268             if((eff.time_increment < EPSILON) ||
00269                 !succ.satisfies(eff.cond_overall)) {
00270                 succ.scheduled_effects.erase(succ.scheduled_effects.begin() + i);
00271                 i--;
00272             }
00273         }
00274     }
00275 
00276     // The scheduled module effects of the new state are precisely the
00277     // scheduled module effects of the predecessor state minus those
00278     // whose scheduled time point has been reached and minus those
00279     // whose over-all condition is violated.
00280     for(int i = 0; i < succ.scheduled_module_effects.size(); i++) {
00281         succ.scheduled_module_effects[i].time_increment -= time_diff;
00282     }
00283     if(!go_to_intermediate_between_now_and_next_happening) {
00284         for(int i = 0; i < succ.scheduled_module_effects.size(); i++) {
00285             const ScheduledModuleEffect &mod_eff = succ.scheduled_module_effects[i];
00286             if((mod_eff.time_increment < EPSILON) ||
00287                 !succ.satisfies(mod_eff.cond_overall)) {
00288                 succ.scheduled_module_effects.erase(
00289                         succ.scheduled_module_effects.begin() + i);
00290                 i--;
00291             }
00292         }
00293     }
00294 
00295     // The persistent over-all conditions of the new state are
00296     // precisely those persistent over-all conditions of the predecessor
00297     // state whose end time-point is properly in the future (not now)
00298     for(int i = 0; i < succ.conds_over_all.size(); i++) {
00299         succ.conds_over_all[i].time_increment -= time_diff;
00300     }
00301     if(!go_to_intermediate_between_now_and_next_happening) {
00302         for(int i = 0; i < succ.conds_over_all.size(); i++) {
00303             const ScheduledCondition &cond = succ.conds_over_all[i];
00304             if((cond.time_increment < EPSILON)) {
00305                 succ.conds_over_all.erase(succ.conds_over_all.begin() + i);
00306                 i--;
00307             }
00308         }
00309     }
00310 
00311     // The persistent at-end conditions of the new state are
00312     // precisely those persistent at-end conditions of the predecessor
00313     // state whose end time-point is in the future
00314     for(int i = 0; i < succ.conds_at_end.size(); i++) {
00315         succ.conds_at_end[i].time_increment -= time_diff;
00316     }
00317     if(!go_to_intermediate_between_now_and_next_happening) {
00318         for(int i = 0; i < succ.conds_at_end.size(); i++) {
00319             const ScheduledCondition &cond = succ.conds_at_end[i];
00320             if(cond.time_increment < EPSILON) {
00321                 succ.conds_at_end.erase(succ.conds_at_end.begin() + i);
00322                 i--;
00323             }
00324         }
00325     }
00326 
00327     // The running operators of the new state are precisely those
00328     // running operators of the predecessor state whose end time-point
00329     // is in the future
00330     for(int i = 0; i < succ.operators.size(); i++) {
00331         succ.operators[i].time_increment -= time_diff;
00332     }
00333     if(!go_to_intermediate_between_now_and_next_happening) {
00334         for(int i = 0; i < succ.operators.size(); i++) {
00335             const ScheduledOperator &op = succ.operators[i];
00336             if((op.time_increment < EPSILON) || op.time_increment <= 0) {
00337                 succ.operators.erase(succ.operators.begin() + i);
00338                 i--;
00339             }
00340         }
00341     }
00342 
00343     succ.initialize();
00344 
00345     return succ;
00346 }
00347 
00348 TimeStampedState TimeStampedState::increase_time_stamp_by(double increment) const
00349 {
00350     TimeStampedState result(*this);
00351     TimeStampedState* res = &result;
00352     while(res->timestamp < timestamp + increment) {
00353         double old_timestamp = res->timestamp;
00354         result = res->let_time_pass(false, false);
00355         res = &result;
00356         double new_timestamp = res->timestamp;
00357         if(double_equals(old_timestamp, new_timestamp))
00358             break;
00359     }
00360     assert(res->timestamp+EPSILON+EPS_TIME >= timestamp + increment);
00361     return *res;
00362 }
00363 //    if(!(res->timestamp+EPSILON >= timestamp + increment)) {
00364 //      cout << "res->timestamp: " << res->timestamp << ", timestamp: " << timestamp << ", increment: " << increment << endl;
00365 //      cout << "running ops:" << endl;
00366 //      for(int i = 0; i < res->operators.size(); ++i) {
00367 //          cout << "increment: " << res->operators[i].time_increment << ", op: ";
00368 //          res->operators[i].dump();
00369 //      }
00370 //      cout << "scheduled effects: " << endl;
00371 //      for(int i = 0; i < res->scheduled_effects.size(); i++)
00372 //          cout << "increment: " << res->scheduled_effects[i].time_increment << " var: " << res->scheduled_effects[i].var << ", pre: " << res->scheduled_effects[i].pre
00373 //              << ", var_post: " << res->scheduled_effects[i].var_post << ", post: " << res->scheduled_effects[i].post << endl;
00374 //      assert(false);
00375 //    }
00376 //    return *res;
00377 //}
00378 
00379 double TimeStampedState::next_happening() const
00380 {
00381     double result = REALLYBIG;
00382     for(int i = 0; i < operators.size(); i++)
00383         if(operators[i].time_increment > 0)
00384             result = min(result, operators[i].time_increment);
00385     if(result == REALLYBIG)
00386         result = 0.0;
00387     return result + timestamp;
00388 }
00389 
00390 void TimeStampedState::dump(bool verbose) const
00391 {
00392     cout << "State (Timestamp: " << timestamp << ")" << endl;
00393     if(verbose) {
00394         cout << " logical state:" << endl;
00395         const int varsPerLine = 10;
00396         unsigned int numCostVars = 0; // dont print cost vars, they are -4 anyways FIXME: do also for other module vars?
00397         unsigned int numPrinted = 0;
00398         for(int i = 0; i < state.size(); i++) {
00399             if(g_variable_types[i] == costmodule) {
00400                 numCostVars++;
00401             } else {
00402                 cout << "  " << g_variable_name[i] << ": " << state[i] << "    ";
00403                 numPrinted++;
00404             }
00405             if(g_variable_types[i] != costmodule) {
00406                 if(numPrinted % varsPerLine == (varsPerLine - 1)) {
00407                     cout << endl;
00408                 }
00409             }
00410         }
00411         cout << "#Cost vars: " << numCostVars << endl;
00412 
00413         cout << endl;
00414         cout << " scheduled effects:" << endl;
00415         for(int i = 0; i < scheduled_effects.size(); i++) {
00416             cout << "  <" << (scheduled_effects[i].time_increment + timestamp) << ",<";
00417             for(int j = 0; j < scheduled_effects[i].cond_overall.size(); j++) {
00418                 cout << g_variable_name[scheduled_effects[i].cond_overall[j].var]
00419                     << ": " << scheduled_effects[i].cond_overall[j].prev;
00420             }
00421             cout << ">,<";
00422             for(int j = 0; j < scheduled_effects[i].cond_end.size(); j++) {
00423                 cout << g_variable_name[scheduled_effects[i].cond_end[j].var]
00424                     << ": " << scheduled_effects[i].cond_end[j].prev;
00425             }
00426             cout << ">,<";
00427             cout << g_variable_name[scheduled_effects[i].var] << " ";
00428             if(is_functional(scheduled_effects[i].var)) {
00429                 cout << scheduled_effects[i].fop << " ";
00430                 cout << g_variable_name[scheduled_effects[i].var_post] << ">>" << endl;
00431             } else {
00432                 cout << ":= ";
00433                 cout << scheduled_effects[i].post << ">>" << endl;
00434             }
00435         }
00436         cout << " persistent over-all conditions:" << endl;
00437         for(int i = 0; i < conds_over_all.size(); i++) {
00438             cout << "  <" << (conds_over_all[i].time_increment + timestamp) << ",<";
00439             cout << g_variable_name[conds_over_all[i].var] << ":"
00440                 << conds_over_all[i].prev << ">>" << endl;
00441         }
00442         cout << " persistent at-end conditions:" << endl;
00443         for(int i = 0; i < conds_at_end.size(); i++) {
00444             cout << "  <" << (conds_at_end[i].time_increment + timestamp) << ",<";
00445             cout << g_variable_name[conds_at_end[i].var] << ":"
00446                 << conds_at_end[i].prev << ">>" << endl;
00447         }
00448         cout << " running operators:" << endl;
00449         for(int i = 0; i < operators.size(); i++) {
00450             cout << "  <" << (operators[i].time_increment + timestamp) << ",<";
00451             cout << operators[i].get_name() << ">>" << endl;
00452         }
00453     }
00454 }
00455 
00456 void TimeStampedState::scheduleEffect(ScheduledEffect effect)
00457 {
00458     scheduled_effects.push_back(effect);
00459     sort(scheduled_effects.begin(), scheduled_effects.end());
00460 }
00461 
00462 bool TimeStampedState::is_consistent_now() const
00463 {
00464     // Persistent over-all conditions must be satisfied
00465     for(int i = 0; i < conds_over_all.size(); i++)
00466         if(!satisfies(conds_over_all[i]))
00467             return false;
00468 
00469     // Persistent at-end conditions must be satisfied
00470     // if their end time point is now
00471     for(int i = 0; i < conds_at_end.size(); i++)
00472         if(double_equals(conds_at_end[i].time_increment, 0) &&
00473             !satisfies(conds_at_end[i]))
00474             return false;
00475 
00476     // No further conditions (?)
00477     return true;
00478 }
00479 
00480 bool TimeStampedState::is_consistent_when_progressed(TimedSymbolicStates* timedSymbolicStates) const
00481 {
00482     double last_time = -1.0;
00483     double current_time = timestamp;
00484     TimeStampedState current_progression(*this);
00485 
00486     bool go_to_intermediate = true;
00487     while(!double_equals(current_time, last_time)) {
00488         if(!current_progression.is_consistent_now()) {
00489             return false;
00490         }
00491 
00492         current_progression = current_progression.let_time_pass(go_to_intermediate, false);
00493         go_to_intermediate = !go_to_intermediate;
00494         last_time = current_time;
00495         current_time = current_progression.timestamp;
00496 
00497         if(!go_to_intermediate && timedSymbolicStates != NULL) {
00498             timedSymbolicStates->push_back(make_pair(vector<double> (), current_progression.timestamp));
00499             for(int i = 0; i < current_progression.state.size(); ++i) {
00500                 if(g_variable_types[i] == primitive_functional || g_variable_types[i] == logical) {
00501                     timedSymbolicStates->back().first.push_back(current_progression.state[i]);
00502                 }
00503             }
00504         }
00505     }
00506 
00507     return true;
00508 }
00509 
00510 TimeStampedState &buildTestState(TimeStampedState &state)
00511 {
00512     vector<Prevail> cas;
00513     vector<Prevail> coa;
00514     vector<Prevail> cae;
00515     state.scheduleEffect(ScheduledEffect(1.0, cas, coa, cae, 11, 0, increase));
00516     state.scheduleEffect(ScheduledEffect(1.0, cas, coa, cae, 9, 0, assign));
00517     return state;
00518 }


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