ompl/control/planners/ltl/src/LTLPlanner.cpp
00001 #include "ompl/control/planners/ltl/LTLPlanner.h"
00002 #include "ompl/control/planners/PlannerIncludes.h"
00003 #include "ompl/control/planners/ltl/ProductGraph.h"
00004 #include "ompl/control/planners/ltl/LTLProblemDefinition.h"
00005 #include "ompl/datastructures/PDF.h"
00006 #include "ompl/util/Console.h"
00007 #include <algorithm>
00008 #include <boost/unordered_map.hpp>
00009 #include <limits>
00010 #include <map>
00011 #include <vector>
00012 
00013 #include <stdio.h>
00014 
00015 ompl::control::LTLPlanner::LTLPlanner(const LTLSpaceInformationPtr& ltlsi, const ProductGraphPtr& a, double exploreTime) :
00016     ompl::base::Planner(ltlsi, "LTLPlanner"),
00017     ltlsi_(ltlsi.get()),
00018     abstraction_(a),
00019     prodStart_(NULL),
00020     exploreTime_(exploreTime)
00021 {
00022     specs_.approximateSolutions = true;
00023 }
00024 
00025 ompl::control::LTLPlanner::~LTLPlanner(void)
00026 {
00027     clearMotions();
00028 }
00029 
00030 void ompl::control::LTLPlanner::setup()
00031 {
00032     base::Planner::setup();
00033 }
00034 
00035 void ompl::control::LTLPlanner::clear()
00036 {
00037     base::Planner::clear();
00038     availDist_.clear();
00039     abstractInfo_.clear();
00040     clearMotions();
00041 }
00042 
00043 ompl::base::PlannerStatus ompl::control::LTLPlanner::solve(const ompl::base::PlannerTerminationCondition& ptc)
00044 {
00045     // TODO make solve work when called more than once!
00046     checkValidity();
00047     const base::State* start = pis_.nextStart();
00048     prodStart_ = ltlsi_->getProdGraphState(start);
00049 
00050     if (pis_.haveMoreStartStates())
00051         OMPL_WARN("Multiple start states given. Using only the first start state.");
00052 
00053     Motion* startMotion = new Motion(ltlsi_);
00054     si_->copyState(startMotion->state, start);
00055     ltlsi_->nullControl(startMotion->control);
00056     startMotion->abstractState = prodStart_;
00057 
00058     motions_.push_back(startMotion);
00059     abstractInfo_[prodStart_].addMotion(startMotion);
00060     updateWeight(prodStart_);
00061     availDist_.add(prodStart_, abstractInfo_[prodStart_].weight);
00062 
00063     abstraction_->buildGraph(prodStart_, boost::bind(&LTLPlanner::initAbstractInfo, this, _1));
00064 
00065     if (!sampler_)
00066         sampler_ = si_->allocStateSampler();
00067     if (!controlSampler_)
00068         controlSampler_ = ltlsi_->allocControlSampler();
00069 
00070     bool solved = false;
00071     Motion* soln;
00072 
00073     while (ptc()==false && !solved)
00074     {
00075         const std::vector<ProductGraph::State*> lead = abstraction_->computeLead(prodStart_, boost::bind(&LTLPlanner::abstractEdgeWeight, this, _1, _2));
00076         buildAvail(lead);
00077         solved = explore(lead, soln, exploreTime_);
00078     }
00079 
00080     if (solved)
00081     {
00082         //build solution path
00083         std::vector<Motion*> path;
00084         while (soln != NULL)
00085         {
00086             path.push_back(soln);
00087             soln = soln->parent;
00088         }
00089         PathControl* pc = new PathControl(si_);
00090         for (int i = path.size()-1; i >= 0; --i)
00091         {
00092             if (path[i]->parent != NULL) {
00093                 pc->append(path[i]->state, path[i]->control, path[i]->steps * ltlsi_->getPropagationStepSize());
00094             }
00095             else {
00096                 pc->append(path[i]->state);
00097             }
00098         }
00099         pdef_->addSolutionPath(base::PathPtr(pc));
00100     }
00101 
00102     OMPL_INFORM("Created %u states", motions_.size());
00103     return base::PlannerStatus(solved, false);
00104 }
00105 
00106 void ompl::control::LTLPlanner::getTree(std::vector<base::State*>& tree) const
00107 {
00108     tree.resize(motions_.size());
00109     for (unsigned int i = 0; i < motions_.size(); ++i)
00110         tree[i] = motions_[i]->state;
00111 }
00112 
00113 std::vector<ompl::control::ProductGraph::State*> ompl::control::LTLPlanner::getHighLevelPath(const std::vector<base::State*>& path, ProductGraph::State* start) const
00114 {
00115     std::vector<ProductGraph::State*> hlPath(path.size());
00116     hlPath[0] = (start != NULL ? start : ltlsi_->getProdGraphState(path[0]));
00117     for (unsigned int i = 1; i < path.size(); ++i)
00118     {
00119         hlPath[i] = ltlsi_->getProdGraphState(path[i]);
00120         if (!hlPath[i]->isValid())
00121             OMPL_WARN("High-level path fails automata");
00122     }
00123     return hlPath;
00124 }
00125 
00126 ompl::control::LTLPlanner::Motion::Motion(void) : state(NULL), control(NULL), parent(NULL), steps(0)
00127 {
00128 }
00129 
00130 ompl::control::LTLPlanner::Motion::Motion(const SpaceInformation* si) :
00131     state(si->allocState()),
00132     control(si->allocControl()),
00133     parent(NULL),
00134     steps(0)
00135 {
00136 }
00137 
00138 ompl::control::LTLPlanner::Motion::~Motion(void)
00139 {
00140 }
00141 
00142 ompl::control::LTLPlanner::ProductGraphStateInfo::ProductGraphStateInfo(void) :
00143     numSel(0),
00144     pdfElem(NULL)
00145 {
00146 }
00147 
00148 void ompl::control::LTLPlanner::ProductGraphStateInfo::addMotion(Motion* m)
00149 {
00150     motionElems[m] = motions.add(m, 1.);
00151 }
00152 
00153 double ompl::control::LTLPlanner::updateWeight(ProductGraph::State* as)
00154 {
00155     ProductGraphStateInfo& info = abstractInfo_[as];
00156     /* TODO weight should include freeVolume, for cases in which decomposition
00157        does not respect obstacles. */
00158     info.weight = ((info.motions.size()+1)*info.volume) / (info.autWeight*(info.numSel+1)*(info.numSel+1));
00159     return info.weight;
00160 }
00161 
00162 void ompl::control::LTLPlanner::initAbstractInfo(ProductGraph::State* as)
00163 {
00164     ProductGraphStateInfo& info = abstractInfo_[as];
00165     info.numSel = 0;
00166     info.pdfElem = NULL;
00167         info.volume = abstraction_->getRegionVolume(as);
00168         unsigned int autDist = std::max(abstraction_->getCosafeAutDistance(as),
00169                 abstraction_->getSafeAutDistance(as));
00170     //TODO try something larger than epsilon
00171     if (autDist == 0)
00172         info.autWeight = std::numeric_limits<double>::epsilon();
00173     else
00174         info.autWeight = autDist;
00175     info.weight = info.volume/info.autWeight;
00176 }
00177 
00178 void ompl::control::LTLPlanner::buildAvail(const std::vector<ProductGraph::State*>& lead)
00179 {
00180     for (unsigned int i = 0; i < availDist_.size(); ++i)
00181         abstractInfo_[availDist_[i]].pdfElem = NULL;
00182     availDist_.clear();
00183     unsigned int numTreePts = 1;
00184     for (int i = lead.size()-1; i >= 0; --i)
00185     {
00186         ProductGraph::State* as = lead[i];
00187         ProductGraphStateInfo& info = abstractInfo_[as];
00188         if (!info.motions.empty())
00189         {
00190             info.pdfElem = availDist_.add(as, info.weight);
00191             numTreePts += info.motions.size();
00192             if (rng_.uniform01() < 0.5)
00193                 break;
00194         }
00195     }
00196 }
00197 
00198 bool ompl::control::LTLPlanner::explore(const std::vector<ProductGraph::State*>& lead, Motion*& soln, double duration)
00199 {
00200     bool solved = false;
00201     base::PlannerTerminationCondition ptc = base::timedPlannerTerminationCondition(duration);
00202     base::GoalPtr goal = pdef_->getGoal();
00203     while (!ptc() && !solved)
00204     {
00205         ProductGraph::State* as = availDist_.sample(rng_.uniform01());
00206         ++abstractInfo_[as].numSel;
00207         updateWeight(as);
00208 
00209         PDF<Motion*>& motions = abstractInfo_[as].motions;
00210         Motion* v = motions.sample(rng_.uniform01());
00211         PDF<Motion*>::Element* velem = abstractInfo_[as].motionElems[v];
00212         double vweight = motions.getWeight(velem);
00213         if (vweight > 1e-20)
00214             motions.update(velem, vweight/(vweight+1.));
00215 
00216         Control* rctrl = ltlsi_->allocControl();
00217         controlSampler_->sampleNext(rctrl, v->control, v->state);
00218         unsigned int cd = controlSampler_->sampleStepCount(ltlsi_->getMinControlDuration(), ltlsi_->getMaxControlDuration());
00219 
00220         base::State* newState = si_->allocState();
00221         cd = ltlsi_->propagateWhileValid(v->state, rctrl, cd, newState);
00222         if (cd < ltlsi_->getMinControlDuration())
00223         {
00224             si_->freeState(newState);
00225             ltlsi_->freeControl(rctrl);
00226                         continue;
00227         }
00228         Motion* m = new Motion();
00229         m->state = newState;
00230         m->control = rctrl;
00231         m->steps = cd;
00232         m->parent = v;
00233         // Since the state was determined to be valid by SpaceInformation, we don't need to check automaton states
00234         m->abstractState = ltlsi_->getProdGraphState(m->state);
00235         motions_.push_back(m);
00236 
00237         abstractInfo_[m->abstractState].addMotion(m);
00238         updateWeight(m->abstractState);
00239         // update weight if hl state already exists in avail
00240         if (abstractInfo_[m->abstractState].pdfElem != NULL)
00241             availDist_.update(abstractInfo_[m->abstractState].pdfElem, abstractInfo_[m->abstractState].weight);
00242         else
00243         {
00244             // otherwise, only add hl state to avail if it already exists in lead
00245                         if (std::find(lead.begin(), lead.end(), m->abstractState) != lead.end())
00246             {
00247                 PDF<ProductGraph::State*>::Element* elem = availDist_.add(m->abstractState, abstractInfo_[m->abstractState].weight);
00248                 abstractInfo_[m->abstractState].pdfElem = elem;
00249             }
00250         }
00251 
00252         solved = goal->isSatisfied(m->state);
00253         if (solved)
00254         {
00255             soln = m;
00256             break;
00257         }
00258     }
00259     return solved;
00260 }
00261 
00262 double ompl::control::LTLPlanner::abstractEdgeWeight(ProductGraph::State* a, ProductGraph::State* b) const
00263 {
00264     const ProductGraphStateInfo& infoA = abstractInfo_.find(a)->second;
00265     const ProductGraphStateInfo& infoB = abstractInfo_.find(b)->second;
00266     return 1./(infoA.weight * infoB.weight);
00267 }
00268 
00269 void ompl::control::LTLPlanner::clearMotions(void)
00270 {
00271     availDist_.clear();
00272     for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i)
00273     {
00274         Motion* m = *i;
00275         if (m->state != NULL)
00276             si_->freeState(m->state);
00277         if (m->control != NULL)
00278             ltlsi_->freeControl(m->control);
00279         delete m;
00280     }
00281     motions_.clear();
00282     pis_.clear();
00283     pis_.update();
00284 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines