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(<LPlanner::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(<LPlanner::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 }