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;
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 ¶ms = 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
00055
00056
00057
00058
00059
00060 double sep = (g_parameters.epsilonize_internally ? EPS_TIME : 0.0);
00061
00062 timestamp = predecessor.timestamp + sep;
00063
00064
00065 double duration = op.get_duration(&predecessor);
00066
00067
00068
00069
00070
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
00079
00080
00081
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
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
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
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
00115
00116
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
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
00150
00151
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
00158
00159
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
00165
00166
00167 operators.push_back(ScheduledOperator(duration, op));
00168
00169
00170
00171
00172
00173 initialize();
00174 }
00175
00176 double TimeStampedState::eps_time(double offset) const {
00177
00178
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
00201
00202
00203
00204
00205
00206
00207 TimeStampedState succ(*this);
00208
00209
00210
00211
00212
00213
00214
00215
00216
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
00236
00237
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
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
00259
00260
00261
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
00277
00278
00279
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
00296
00297
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
00312
00313
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
00328
00329
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
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
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;
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
00465 for(int i = 0; i < conds_over_all.size(); i++)
00466 if(!satisfies(conds_over_all[i]))
00467 return false;
00468
00469
00470
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
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 }