partial_order_lifter.cpp
Go to the documentation of this file.
00001 #include "partial_order_lifter.h"
00002 
00003 #include<iomanip>
00004 
00005 void InstantPlanStep::print_name()
00006 {
00007     if (type == dummy_start_action) {
00008         cout << "dummy_start_action";
00009     } else if (type == dummy_end_action) {
00010         cout << "dummy_end_action";
00011     } else {
00012         cout << name;
00013     }
00014 }
00015 
00016 void InstantPlanStep::dump()
00017 {
00018     print_name();
00019     cout << endl;
00020     cout << "     Timepoint:" << timepoint << endl;
00021     cout << "     precond vars:" << endl;
00022     set<int>::iterator it;
00023     for (it = precondition_vars.begin(); it != precondition_vars.end(); ++it) {
00024         cout << "      " << *it << endl;
00025     }
00026     cout << "     effect vars:" << endl;
00027     for (it = effect_vars.begin(); it != effect_vars.end(); ++it) {
00028         cout << "      " << *it << endl;
00029     }
00030     cout << "     effect_cond_vars:" << endl;
00031     for (it = effect_cond_vars.begin(); it != effect_cond_vars.end(); ++it) {
00032         cout << "      " << *it << endl;
00033     }
00034     cout << "     effects:" << endl;
00035     for (int i = 0; i < effects.size(); ++i) {
00036         cout << "      " << effects[i].var << ": " << effects[i].post << endl;
00037     }
00038     cout << "     preconditions:" << endl;
00039     for (int i = 0; i < preconditions.size(); ++i) {
00040         cout << "      " << preconditions[i].var << ": "
00041             << preconditions[i].prev << endl;
00042     }
00043     cout << "     overall conds:" << endl;
00044     for (int i = 0; i < overall_conds.size(); ++i) {
00045         cout << "      " << overall_conds[i].var << ": "
00046             << overall_conds[i].prev << endl;
00047     }
00048     if (endAction != -1) {
00049         cout << "     endAction: " << endAction << endl;
00050     }
00051     if (actionFinishingImmeadatlyAfterThis) {
00052         cout << "     actionFinishingImmeadatlyAfterThis: "
00053             << actionFinishingImmeadatlyAfterThis << endl;
00054     }
00055 }
00056 
00057 void PartialOrderLifter::sortPlan(Plan& plan)
00058 {
00059     bool changePerformed = false;
00060     do {
00061         changePerformed = false;
00062         for (int i = 0; i < plan.size() - 1; ++i) {
00063             if (plan[i].start_time - EPSILON > plan[i + 1].start_time) {
00064                 swap(plan[i], plan[i + 1]);
00065                 changePerformed = true;
00066             }
00067         }
00068     } while (changePerformed);
00069 }
00070 
00071 Plan PartialOrderLifter::createAndSolveSTN()
00072 {
00073     vector<string> variable_names;
00074     for (int i = 0; i < instant_plan.size(); ++i) {
00075         variable_names.push_back(instant_plan[i].name);
00076     }
00077 
00078     SimpleTemporalProblem stn(variable_names);
00079 
00080     for (int i = 0; i < instant_plan.size(); ++i) {
00081         if (instant_plan[i].type == start_action) {
00082             assert(instant_plan[i].endAction != -1);
00083             // assert that start time point of actions are non-negative
00084             stn.setUnboundedIntervalFromXZero(i, 0.0);
00085             // assert that differences between start and end time points are exactly
00086             // the durations of the actions
00087             stn.setSingletonInterval(i, instant_plan[i].endAction,
00088                     instant_plan[i].duration);
00089         } else {
00090             assert(instant_plan[i].type == end_action);
00091             // assert that two actions ending at the same time point in the original plan
00092             // also do so in the scheduled one
00093             if (instant_plan[i].actionFinishingImmeadatlyAfterThis != -1) {
00094                 //                stn.setSingletonInterval(i,instant_plan[i].actionFinishingImmeadatlyAfterThis,EPSILON);
00095             }
00096         }
00097     }
00098 
00099     // assert that causal relationships are preserved
00100     for (set<Ordering>::iterator it = partial_order.begin(); it
00101             != partial_order.end(); ++it) {
00102         stn.setUnboundedInterval(it->first, it->second, EPSILON);
00103     }
00104 
00105     //    // assert that overall conditions are preserved
00106     //    for(int i = 0; i < plan.size(); ++i) {
00107     //        const vector<Prevail>& overall_conds = plan[i].op->get_prevail_overall();
00108     //        for(int j = 0; j < overall_conds.size(); ++j) {
00109     //            for(int k = 0; k < instant_plan.size(); ++k) {
00110     //                InstantPlanStep& step = instant_plan[k];
00111     //                for(int l = 0; l < step.effects.size(); ++l) {
00112     //                    if(overall_conds[j].var == step.effects[l].var && overall_conds[j].prev != step.effects[l].post) {
00113     //                        if(plan[i].start_time < step.start_time) {
00114     //                            stn.st
00115     //                        }
00116     //                    }
00117     //                }
00118     //            }
00119     //        }
00120     //
00121     //    }
00122 
00123     //    std::cout << "Unsolved Simple Temporal Network:" << std::endl;
00124     //
00125     //
00126     //    std::cout << "=================================" << std::endl;
00127     //    stn.dump();
00128 
00129     //    stn.solve();
00130     bool isValid = stn.solveWithP3C();
00131     assert(isValid);
00132     vector<Happening> happenings = stn.getHappenings(true);
00133 
00134     map<string, int> startedActions;
00135 
00136     for (int i = 0; i < happenings.size(); ++i) {
00137         if (instant_plan[happenings[i].second].type == start_action) {
00138             assert(instant_plan[happenings[i].second].correspondingPlanStep < instant_plan.size());
00139             plan[instant_plan[happenings[i].second].correspondingPlanStep].start_time
00140                 = happenings[i].first;
00141         }
00142     }
00143 
00144     sortPlan(plan);
00145 
00146     return plan;
00147 
00148     //    double h = stn.getMaximalTimePointInTightestSchedule();
00149     //
00150     //    std::cout << "Solved Simple Temporal Network:" << std::endl;
00151     //    std::cout << "===============================" << std::endl;
00152     //    stn.dump();
00153     //
00154     //    std::cout << "Corresponding happenings:" << std::endl;
00155     //    std::cout << "=========================" << std::endl;
00156     //    stn.dumpSolution();
00157     //
00158     //    std::cout << "Corresponding heuristic value:" << std::endl;
00159     //    std::cout << "==============================" << std::endl;
00160     //    std::cout << h << std::endl;
00161 
00162 }
00163 
00164 Plan PartialOrderLifter::lift()
00165 {
00166     //    instant_plan.clear();
00167     //    partial_order.clear();
00168     buildInstantPlan();
00169     //    dumpInstantPlan();
00170     buildPartialOrder();
00171     //    dumpOrdering();
00172     return createAndSolveSTN();
00173 }
00174 
00175 void PartialOrderLifter::buildPartialOrder()
00176 {
00177     std::vector<std::vector<Prevail> > primary_add;
00178     primary_add.resize(instant_plan.size());
00179 
00180     for (int i = instant_plan.size() - 1; i >= 0; --i) {
00181         //        cout << "Looking at op: ";
00182         //        instant_plan[i].print_name();
00183         //        cout << endl;
00184 
00185         // step (a) find achievers for all action preconditions and add corresponding ordering constraints
00186         InstantPlanStep &step = instant_plan[i];
00187         for (int j = 0; j < step.preconditions.size(); ++j) {
00188             //            cout << "  Search achiever for ";
00189             //            step.preconditions[j].dump();
00190             //            cout << endl;
00191             bool achiever_found = false;
00192             for (int k = i - 1; k >= 0; --k) {
00193                 if (!achiever_found) {
00194                     InstantPlanStep &temp_step = instant_plan[k];
00195                     for (int l = 0; l < temp_step.effects.size(); ++l) {
00196                         //                        cout << " " << step.preconditions[j].var << " " << temp_step.effects[l].var << " " << step.preconditions[j].prev << " " << temp_step.effects[l].post << endl;
00197                         if (step.preconditions[j].var
00198                                 == temp_step.effects[l].var
00199                                 && step.preconditions[j].prev
00200                                 == temp_step.effects[l].post && step.op
00201                                 != temp_step.op) {
00202                             // achiever found!
00203                             //                            cout << "   achiever found: ";
00204                             //                            temp_step.print_name();
00205                             //                            cout << endl;
00206                             partial_order.insert(make_pair(k, i));
00207                             primary_add[k].push_back(step.preconditions[j]);
00208                             achiever_found = true;
00209                             break;
00210                         }
00211                     }
00212                 } else {
00213                     achiever_found = false;
00214                     break;
00215                 }
00216             }
00217         }
00218 
00219         // step (b) demote actions threatening preconditions
00220         for (int j = 0; j < step.effects.size(); ++j) {
00221             for (int k = i - 1; k >= 0; --k) {
00222                 InstantPlanStep &temp_step = instant_plan[k];
00223                 for (int l = 0; l < temp_step.preconditions.size(); ++l) {
00224                     if (temp_step.preconditions[l].var == step.effects[j].var
00225                             && temp_step.preconditions[l].prev
00226                             != step.effects[j].post && step.op
00227                             != temp_step.op) {
00228                         //                        step.print_name();
00229                         //                        cout << " threatens ";
00230                         //                        temp_step.print_name();
00231                         //                        cout << endl;
00232                         partial_order.insert(make_pair(k, i));
00233                     }
00234                 }
00235             }
00236         }
00237 
00238         // mutex conditions: no two actions may affect the same variable at the same time!
00239         for (int j = 0; j < step.effects.size(); ++j) {
00240             for (int k = i - 1; k >= 0; --k) {
00241                 InstantPlanStep &temp_step = instant_plan[k];
00242                 for (int l = 0; l < temp_step.effects.size(); ++l) {
00243                     if (temp_step.effects[l].var == step.effects[j].var
00244                             && temp_step.effects[l].post
00245                             != step.effects[j].post && step.op
00246                             != temp_step.op) {
00247                         partial_order.insert(make_pair(k, i));
00248                     }
00249                 }
00250             }
00251         }
00252 
00253         // step (c)
00254         for (int j = 0; j < primary_add[i].size(); ++j) {
00255             for (int k = i - 1; k >= 0; --k) {
00256                 InstantPlanStep &temp_step = instant_plan[k];
00257                 for (int l = 0; l < temp_step.effects.size(); ++l) {
00258                     if (temp_step.effects[l].var == primary_add[i][j].var
00259                             && temp_step.effects[l].post
00260                             != primary_add[i][j].prev && step.op
00261                             != temp_step.op) {
00262                         //                        step.print_name();
00263                         //                        cout << " threatens primary add of ";
00264                         //                        temp_step.print_name();
00265                         //                        cout << endl;
00266                         partial_order.insert(make_pair(k, i));
00267                     }
00268                 }
00269             }
00270         }
00271 
00272         // overallconds must be satiesfied!
00273         for (int j = 0; j < step.overall_conds.size(); ++j) {
00274             for (int k = 0; k < instant_plan.size(); ++k) {
00275                 InstantPlanStep &temp_step = instant_plan[k];
00276                 for (int l = 0; l < temp_step.effects.size(); ++l) {
00277                     if (temp_step.effects[l].var == step.overall_conds[j].var
00278                             /*&& temp_step.effects[l].post != step.overall_conds[j].prev*/&& step.op
00279                             != temp_step.op) {
00280                         if (step.timepoint >= temp_step.timepoint && step.type
00281                                 == start_action) {
00282                             //                            temp_step.print_name();
00283                             //                            cout << " could break overall cond of ";
00284                             //                            step.print_name();
00285                             //                            cout << endl;
00286                             partial_order.insert(make_pair(k, i));
00287                         } else if (step.timepoint <= temp_step.timepoint
00288                                 && step.type == end_action) {
00289                             //                            step.print_name();
00290                             //                            cout << " could break overall cond of ";
00291                             //                            temp_step.print_name();
00292                             //                            cout << endl;
00293                             partial_order.insert(make_pair(i, k));
00294                         }
00295                     }
00296                 }
00297 
00298             }
00299         }
00300 
00301     }
00302 
00303     //    fixme();
00304 }
00305 
00306 void PartialOrderLifter::dumpInstantPlan()
00307 {
00308     for (int i = 0; i < instant_plan.size(); ++i) {
00309         cout << i << ": ";
00310         instant_plan[i].dump();
00311         cout << "-----------------------------------" << endl;
00312     }
00313 }
00314 
00315 void PartialOrderLifter::dumpOrdering()
00316 {
00317     cout << "numberOfOrderingConstraints: " << partial_order.size() << endl;
00318     cout << "Ordering:" << endl;
00319     for (std::set<Ordering>::iterator it = partial_order.begin(); it
00320             != partial_order.end(); ++it) {
00321         cout << " ";
00322         instant_plan[it->first].print_name();
00323         cout << " < ";
00324         instant_plan[it->second].print_name();
00325         cout << endl;
00326     }
00327 }
00328 
00329 void PartialOrderLifter::findTriggeringEffects(
00330         const TimeStampedState* stateBeforeHappening,
00331         const TimeStampedState* stateAfterHappening, vector<PrePost>& effects)
00332 {
00333     effects.clear();
00334     assert(stateAfterHappening->state.size() == stateBeforeHappening->state.size());
00335     for (int i = 0; i < stateAfterHappening->state.size(); ++i) {
00336         if (!(double_equals(stateBeforeHappening->state[i],
00337                         stateAfterHappening->state[i]))) {
00338             effects.push_back(PrePost(i, stateAfterHappening->state[i]));
00339         }
00340     }
00341 }
00342 
00343 //void PartialOrderLifter::findTriggeringEffectsForInitialState(const TimeStampedState* tsstate, vector<PrePost>& effects) {
00344 //    for(int i = 0; i < tsstate->state.size(); ++i) {
00346 //    }
00347 //}
00348 
00349 void PartialOrderLifter::findAllEffectCondVars(const ScheduledOperator& new_op,
00350         set<int>& effect_cond_vars, ActionType type)
00351 {
00352     // FIXME: start_type -> start_conds, end_type -> end_conds, overall_conds??
00353     const vector<PrePost>* pre_posts;
00354     if (type == start_action) {
00355         pre_posts = &new_op.get_pre_post_start();
00356     } else {
00357         assert(type == end_action);
00358         pre_posts = &new_op.get_pre_post_end();
00359     }
00360     const vector<Prevail>* prevails;
00361     for (int i = 0; i < pre_posts->size(); ++i) {
00362         if (type == start_action) {
00363             prevails = &(*pre_posts)[i].cond_start;
00364         } else {
00365             assert(type == end_action);
00366             prevails = &(*pre_posts)[i].cond_end;
00367         }
00368         for (int j = 0; j < prevails->size(); ++j) {
00369             effect_cond_vars.insert((*prevails)[i].var);
00370         }
00371     }
00372 }
00373 
00374 void PartialOrderLifter::findPreconditions(const ScheduledOperator& new_op,
00375         vector<Prevail>& preconditions, ActionType type)
00376 {
00377     // FIXME: start_type -> start_conds, end_type -> end_conds, overall_conds??
00378     const std::vector<Prevail> *prevails;
00379     const std::vector<PrePost> *pre_posts;
00380     if (type == start_action) {
00381         prevails = &new_op.get_prevail_start();
00382         pre_posts = &new_op.get_pre_post_start();
00383     } else {
00384         prevails = &new_op.get_prevail_end();
00385         pre_posts = &new_op.get_pre_post_end();
00386     }
00387     for (size_t i = 0; i < prevails->size(); i++) {
00388         preconditions.push_back((*prevails)[i]);
00389     }
00390     for (int i = 0; i < pre_posts->size(); ++i) {
00391         if ((*pre_posts)[i].pre != -1) {
00392             preconditions.push_back(Prevail((*pre_posts)[i].var,
00393                         (*pre_posts)[i].pre));
00394         }
00395     }
00396 }
00397 
00398 int PartialOrderLifter::getIndexOfPlanStep(const ScheduledOperator& op,
00399         double timestamp)
00400 {
00401     for (int i = 0; i < plan.size(); ++i) {
00402         if (plan[i].op->get_name().compare(op.get_name()) == 0
00403                 && double_equals(plan[i].start_time, timestamp)) {
00404             return i;
00405         }
00406     }
00407     assert(false);
00408     return -1;
00409 }
00410 
00411 void PartialOrderLifter::buildInstantPlan()
00412 {
00413     const TimeStampedState* stateBeforeHappening;
00414     const TimeStampedState* stateAfterHappening;
00415     map<double, vector<const ScheduledOperator*> , doubleEquComp>
00416         actionsEndingAtGivenTime;
00417     map<double, list<InstantPlanStep*> , doubleEquComp>
00418         instantActionsStartingAtGivenTime;
00419     list<InstantPlanStep*>::iterator it;
00420     //    instant_plan.push_back(InstantPlanStep(dummy_start_action, 0.0, NULL));
00421     //    instant_plan.push_back(InstantPlanStep(dummy_end_action, trace.back()->timestamp, NULL));
00422     //    assert(instant_plan.size() == 2);
00423     int startPoints = 0;
00424     int endPoints = 0;
00425     for (int i = 0; i < trace.size() - 1; ++i) {
00426         stateBeforeHappening = trace[i];
00427         // effects of dummy start instant is initial state
00428         vector<PrePost> effects;
00429         //        findTriggeringEffectsForInitialState(stateBeforeHappening,effects);
00430         //        instant_plan[0].effects = effects;
00431         stateAfterHappening = trace[i + 1];
00432         findTriggeringEffects(stateBeforeHappening, stateAfterHappening,
00433                 effects);
00434         double currentTimeStamp = stateAfterHappening->timestamp;
00435         if (stateBeforeHappening->operators.size()
00436                 < stateAfterHappening->operators.size()) {
00437             // A new action starts at this happening!
00438             startPoints++;
00439             assert(stateBeforeHappening->operators.size() + 1 == stateAfterHappening->operators.size());
00440             if (!(double_equals(stateBeforeHappening->timestamp,
00441                             stateAfterHappening->timestamp))) {
00442                 cout << stateBeforeHappening->timestamp << ", "
00443                     << stateAfterHappening->timestamp << endl;
00444                 assert(false);
00445             }
00446             const ScheduledOperator& new_op =
00447                 stateAfterHappening->operators.back();
00448             double startTime = currentTimeStamp;
00449             double time_increment = new_op.time_increment;
00450             double endTime = startTime + time_increment;
00451             vector<Prevail> preconditions;
00452             findPreconditions(new_op, preconditions, start_action);
00453             set<int> effect_cond_vars;
00454             findAllEffectCondVars(new_op, effect_cond_vars, start_action);
00455             actionsEndingAtGivenTime[endTime].push_back(&new_op);
00456 
00457             double duration = new_op.get_duration(stateBeforeHappening);
00458             instant_plan.push_back(InstantPlanStep(start_action, startTime,
00459                         duration, &new_op));
00460             instant_plan.back().effects = effects;
00461             instant_plan.back().effect_cond_vars = effect_cond_vars;
00462             instant_plan.back().preconditions = preconditions;
00463             instant_plan.back().overall_conds = new_op.get_prevail_overall();
00464             instant_plan.back().correspondingPlanStep = getIndexOfPlanStep(
00465                     new_op, startTime);
00466             //            assert(instant_plan.size() >= 3);
00467             instantActionsStartingAtGivenTime[startTime].push_back(
00468                     &(instant_plan.back()));
00469             //            // Insert ordering constraint between dummy_start and current start
00470             //            assert(instant_plan[0].type == dummy_start_action);
00471             //            partial_order.push_back(make_pair(&instant_plan[0], &instant_plan.back()));
00472         } else if (stateBeforeHappening->operators.size() >= stateAfterHappening->operators.size()) {
00473             // At least on action ends at this happening!
00474             assert(i> 0);
00475             assert(stateBeforeHappening->operators.size() > stateAfterHappening->operators.size());
00476             assert(stateBeforeHappening->timestamp < stateAfterHappening->timestamp + EPSILON);
00477             InstantPlanStep *lastEndingAction = NULL;
00478             double endTime = currentTimeStamp;
00479             //            endTime = (int)(endTime*1000000)/1000000.0;
00480             for (int j = 0; j < actionsEndingAtGivenTime[endTime].size(); ++j) {
00481                 endPoints++;
00482                 instant_plan.push_back(InstantPlanStep(end_action, endTime, -1,
00483                             actionsEndingAtGivenTime[endTime][j]));
00484                 vector<Prevail> preconditions;
00485                 findPreconditions((*actionsEndingAtGivenTime[endTime][j]),
00486                         preconditions, end_action);
00487                 set<int> effect_cond_vars;
00488                 findAllEffectCondVars((*actionsEndingAtGivenTime[endTime][j]),
00489                         effect_cond_vars, end_action);
00490                 instant_plan.back().effects = effects;
00491                 instant_plan.back().effect_cond_vars = effect_cond_vars;
00492                 instant_plan.back().preconditions = preconditions;
00493                 instant_plan.back().overall_conds
00494                     = (*actionsEndingAtGivenTime[endTime][j]).get_prevail_overall();
00495                 double time_increment =
00496                     actionsEndingAtGivenTime[endTime][j]->time_increment;
00497                 double startingTime = endTime - time_increment;
00498                 bool bad = true;
00499                 for (it
00500                         = instantActionsStartingAtGivenTime[startingTime].begin(); it
00501                         != instantActionsStartingAtGivenTime[startingTime].end(); ++it) {
00502                     if ((*it)->op == instant_plan.back().op) {
00503                         //                       instant_plan.back().correspondingAction = *(it);
00504                         (*it)->endAction = instant_plan.size() - 1;
00505                         bad = false;
00506                         // Insert ordering between corresponding start and end actions.
00507                         //                        partial_order.push_back(make_pair(*(it),&instant_plan.back()));
00508                         // Insert ordering constraint between dummy_end and current end
00509                         //                        assert(instant_plan[1].type == dummy_end_action);
00510                         //                        partial_order.push_back(make_pair(&(instant_plan.back()),&(instant_plan[1])));
00511                     }
00512                 }
00513                 assert(!bad);
00514                 if (lastEndingAction != NULL) {
00515                     // If two actions finish at the same time point, let the one lasting longer finish first
00516                     // by inserting an according ordering constraint
00517 
00518                     //                    partial_order.push_back(make_pair(lastEndingAction,
00519                     //                            &instant_plan.back()));
00520                     lastEndingAction->actionFinishingImmeadatlyAfterThis
00521                         = instant_plan.size() - 1;
00522                 }
00523                 lastEndingAction = &instant_plan.back();
00524             }
00525             actionsEndingAtGivenTime[endTime].clear();
00526         } else {
00527             continue;
00528             //            assert(false);
00529         }
00530         cout << "startPoints: " << startPoints << ", endPoints: " << endPoints << endl;
00531     }
00532     assert(startPoints == endPoints);
00533 
00537     //stable_sort(instant_plan.begin(), instant_plan.end());
00538 }


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