Lightning.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, JSK, The University of Tokyo.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the JSK, The University of Tokyo nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman */
36 
37 #include "ompl/tools/lightning/Lightning.h"
38 #include "ompl/tools/lightning/LightningDB.h"
39 
40 namespace og = ompl::geometric;
41 namespace ob = ompl::base;
42 namespace ot = ompl::tools;
43 
45 {
46  initialize();
47 }
48 
50 {
51  initialize();
52 }
53 
54 void ompl::tools::Lightning::initialize()
55 {
56  // Load dynamic time warp
57  dtw_ = std::make_shared<ot::DynamicTimeWarp>(si_);
58 
59  // Load the experience database
60  experienceDB_ = std::make_shared<ompl::tools::LightningDB>(si_->getStateSpace());
61 
62  // Load the Retrieve repair database. We do it here so that setRepairPlanner() works
63  rrPlanner_ = std::make_shared<og::LightningRetrieveRepair>(si_, experienceDB_);
64 
65  OMPL_INFORM("Lightning Framework initialized.");
66 }
67 
69 {
70  if (!configured_ || !si_->isSetup() || !planner_->isSetup() || !rrPlanner_->isSetup())
71  {
72  OMPL_INFORM("Setting up the Lightning Framework");
73 
74  if (!configured_)
75  OMPL_INFORM(" Setting up because not configured");
76  else if (!si_->isSetup())
77  OMPL_INFORM(" Setting up because not si->isSetup");
78  else if (!planner_->isSetup())
79  OMPL_INFORM(" Setting up because not planner->isSetup");
80  else if (!rrPlanner_->isSetup())
81  OMPL_INFORM(" Setting up because not rrPlanner->isSetup");
82 
83  // Setup Space Information if we haven't already done so
84  if (!si_->isSetup())
85  si_->setup();
86 
87  // Setup planning from scratch planner
88  if (!planner_)
89  {
90  if (pa_)
91  planner_ = pa_(si_);
92  if (!planner_)
93  {
95  pdef_->getGoal()); // we could use the repairProblemDef_ here but that isn't setup yet
96 
97  OMPL_INFORM("No planner specified. Using default: %s", planner_->getName().c_str());
98  }
99  }
100  planner_->setProblemDefinition(pdef_);
101  if (!planner_->isSetup())
102  planner_->setup();
103 
104  // Setup planning from experience planner
105  rrPlanner_->setProblemDefinition(pdef_);
106 
107  if (!rrPlanner_->isSetup())
108  rrPlanner_->setup();
109 
110  // Create the parallel component for splitting into two threads
111  pp_ = std::make_shared<ot::ParallelPlan>(pdef_);
113  {
114  throw Exception("Both planning from scratch and experience have been disabled, unable to plan");
115  }
116  if (scratchEnabled_)
117  pp_->addPlanner(planner_); // Add the planning from scratch planner if desired
118  if (recallEnabled_)
119  pp_->addPlanner(rrPlanner_); // Add the planning from experience planner if desired
120 
121  // Check if experience database is already loaded
122  if (experienceDB_->isEmpty())
123  {
124  if (filePath_.empty())
125  {
126  OMPL_ERROR("No file path has been specified, unable to load experience DB");
127  }
128  else
129  {
130  experienceDB_->load(filePath_); // load from file
131  }
132  }
133  else
134  OMPL_ERROR("Attempting to load experience database when it is not empty");
135 
136  // Set the configured flag
137  configured_ = true;
138  }
139 }
140 
142 {
143  if (planner_)
144  planner_->clear();
145  if (rrPlanner_)
146  rrPlanner_->clear();
147  if (pdef_)
148  pdef_->clearSolutionPaths();
149  if (pp_)
150  {
151  pp_->clearHybridizationPaths();
152  }
153 }
154 
155 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner
156 // termination condition
158 {
159  OMPL_INFORM("Lightning Framework: Starting solve()");
160 
161  // Setup again in case it has not been done yet
162  setup();
163 
165  time::point start = time::now();
166 
167  // Insertion time
168  double insertionTime = 0.;
169 
170  // Start both threads
171  bool hybridize = false;
172  lastStatus_ = pp_->solve(ptc, hybridize);
173 
174  // Planning time
175  planTime_ = time::seconds(time::now() - start);
176  stats_.totalPlanningTime_ += planTime_; // used for averaging
177  stats_.numProblems_++; // used for averaging
178 
179  // Create log
181  log.planning_time = planTime_;
182 
184  {
185  // Skip further processing if absolutely no path is available
186  OMPL_ERROR("Lightning Solve: No solution found after %f seconds", planTime_);
187  stats_.numSolutionsTimedout_++;
188 
189  // Logging
190  log.planner = "neither_planner";
191  log.result = "timedout";
192  log.is_saved = "not_saved";
193  }
194  else if (!lastStatus_)
195  {
196  // Skip further processing if absolutely no path is available
197  OMPL_ERROR("Lightning Solve: Unknown failure, planner status: %s", lastStatus_.asString().c_str());
198  stats_.numSolutionsFailed_++;
199 
200  // Logging
201  log.planner = "neither_planner";
202  log.result = "failed";
203  log.is_saved = "not_saved";
204  }
205  else
206  {
207  OMPL_INFORM("Lightning Solve: Possible solution found in %f seconds", planTime_);
208 
209  // Smooth the result
210  simplifySolution(ptc);
211 
212  og::PathGeometric solutionPath = getSolutionPath(); // copied so that it is non-const
213  OMPL_INFORM("Solution path has %d states and was generated from planner %s", solutionPath.getStateCount(),
214  getSolutionPlannerName().c_str());
215 
216  // Logging
217  log.planner = getSolutionPlannerName();
218 
219  // Do not save if approximate
220  if (!haveExactSolutionPath())
221  {
222  // Logging
223  log.result = "not_exact_solution";
224  log.is_saved = "not_saved";
225  log.approximate = true;
226 
227  // Stats
228  stats_.numSolutionsApproximate_++;
229 
230  // not sure what to do here, use case not tested
231  OMPL_INFORM("NOT saving to database because the solution is APPROXIMATE");
232  }
233  // Use dynamic time warping to see if the repaired path is too similar to the original
234  else if (getSolutionPlannerName() == rrPlanner_->getName())
235  {
236  // Stats
237  stats_.numSolutionsFromRecall_++;
238 
239  // Logging
240  log.result = "from_recall";
241 
242  // Make sure solution has at least 2 states
243  if (solutionPath.getStateCount() < 2)
244  {
245  OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
246  stats_.numSolutionsTooShort_++;
247 
248  // Logging
249  log.is_saved = "less_2_states";
250  log.too_short = true;
251  }
252  else
253  {
254  // Benchmark runtime
255  time::point startTime = time::now();
256 
257  // Convert the original recalled path to PathGeometric
259  og::PathGeometric chosenRecallPath(si_);
260  convertPlannerData(chosenRecallPathData, chosenRecallPath);
261 
262  // Reverse path2 if necessary so that it matches path1 better
263  reversePathIfNecessary(solutionPath, chosenRecallPath);
264 
265  double score = dtw_->getPathsScore(solutionPath, chosenRecallPath);
266  log.score = score;
267 
268  if (score < 4)
269  {
270  OMPL_INFORM("NOT saving to database because best solution was from database and is too similar "
271  "(score %f)",
272  score);
273 
274  // Logging
275  log.insertion_failed = true;
276  log.is_saved = "score_too_similar";
277  }
278  else
279  {
280  OMPL_INFORM("Adding path to database because repaired path is different enough from original "
281  "recalled path (score %f)",
282  score);
283 
284  // Logging
285  log.insertion_failed = false;
286  log.is_saved = "score_different_enough";
287 
288  // Stats
289  stats_.numSolutionsFromRecallSaved_++;
290 
291  // Save to database
292  double dummyInsertionTime; // unused because does not include scoring function
293  experienceDB_->addPath(solutionPath, dummyInsertionTime);
294  }
295  insertionTime += time::seconds(time::now() - startTime);
296  }
297  }
298  else
299  {
300  // Logging
301  log.result = "from_scratch";
302 
303  // Stats
304  stats_.numSolutionsFromScratch_++;
305 
306  // Make sure solution has at least 2 states
307  if (solutionPath.getStateCount() < 2)
308  {
309  OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
310 
311  // Logging
312  log.is_saved = "less_2_states";
313  log.too_short = true;
314 
315  // Stats
316  stats_.numSolutionsTooShort_++;
317  }
318  else
319  {
320  OMPL_INFORM("Adding path to database because best solution was not from database");
321 
322  // Logging
323  log.result = "from_scratch";
324  log.is_saved = "saving";
325 
326  // Save to database
327  experienceDB_->addPath(solutionPath, insertionTime);
328  }
329  }
330  }
331 
332  stats_.totalInsertionTime_ += insertionTime; // used for averaging
333 
334  // Final log data
335  log.insertion_time = insertionTime;
336  log.num_vertices = experienceDB_->getStatesCount();
337  log.num_edges = 0;
338  log.num_connected_components = 0;
339 
340  // Flush the log to buffer
341  convertLogToString(log);
342 
343  return lastStatus_;
344 }
345 
347 {
348  ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(time);
349  return solve(ptc);
350 }
351 
353 {
354  if (filePath_.empty())
355  {
356  OMPL_ERROR("No file path has been specified, unable to save experience DB");
357  return false;
358  }
359  return experienceDB_->save(filePath_);
360 }
361 
363 {
364  if (filePath_.empty())
365  {
366  OMPL_ERROR("No file path has been specified, unable to save experience DB");
367  return false;
368  }
369  return experienceDB_->saveIfChanged(filePath_);
370 }
371 
372 void ompl::tools::Lightning::printResultsInfo(std::ostream &out) const
373 {
374  for (std::size_t i = 0; i < pdef_->getSolutionCount(); ++i)
375  {
376  out << "Solution " << i << " | Length: " << pdef_->getSolutions()[i].length_
377  << " | Approximate: " << (pdef_->getSolutions()[i].approximate_ ? "true" : "false")
378  << " | Planner: " << pdef_->getSolutions()[i].plannerName_ << std::endl;
379  }
380 }
381 
382 void ompl::tools::Lightning::print(std::ostream &out) const
383 {
384  if (si_)
385  {
386  si_->printProperties(out);
387  si_->printSettings(out);
388  }
389  if (planner_)
390  {
391  planner_->printProperties(out);
392  planner_->printSettings(out);
393  }
394  if (rrPlanner_)
395  {
396  rrPlanner_->printProperties(out);
397  rrPlanner_->printSettings(out);
398  }
399  if (pdef_)
400  pdef_->print(out);
401 }
402 
403 void ompl::tools::Lightning::printLogs(std::ostream &out) const
404 {
405  out << "Lightning Framework Logging Results" << std::endl;
406  out << " Solutions Attempted: " << stats_.numProblems_ << std::endl;
407  out << " Solved from scratch: " << stats_.numSolutionsFromScratch_ << " ("
408  << stats_.numSolutionsFromScratch_ / stats_.numProblems_ * 100.0 << "%)" << std::endl;
409  out << " Solved from recall: " << stats_.numSolutionsFromRecall_ << " ("
410  << stats_.numSolutionsFromRecall_ / stats_.numProblems_ * 100.0 << "%)" << std::endl;
411  out << " That were saved: " << stats_.numSolutionsFromRecallSaved_ << std::endl;
412  out << " That were discarded: " << stats_.numSolutionsFromRecall_ - stats_.numSolutionsFromRecallSaved_
413  << std::endl;
414  out << " Less than 2 states: " << stats_.numSolutionsTooShort_ << std::endl;
415  out << " Failed: " << stats_.numSolutionsFailed_ << std::endl;
416  out << " Timedout: " << stats_.numSolutionsTimedout_ << std::endl;
417  out << " Approximate: " << stats_.numSolutionsApproximate_ << std::endl;
418  out << " LightningDb " << std::endl;
419  out << " Total paths: " << experienceDB_->getExperiencesCount() << std::endl;
420  out << " Vertices (states): " << experienceDB_->getStatesCount() << std::endl;
421  out << " Unsaved solutions: " << experienceDB_->getNumUnsavedPaths() << std::endl;
422  out << " Average planning time: " << stats_.getAveragePlanningTime() << std::endl;
423  out << " Average insertion time: " << stats_.getAverageInsertionTime() << std::endl;
424 }
425 
427 {
428  return experienceDB_->getExperiencesCount();
429 }
430 
431 void ompl::tools::Lightning::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
432 {
433  experienceDB_->getAllPlannerDatas(plannerDatas);
434 }
435 
437 {
438  // Convert the planner data verticies into a vector of states
439  for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
440  path.append(plannerData->getVertex(i).getState());
441 }
442 
444 {
445  // Reverse path2 if it matches better
446  const ob::State *s1 = path1.getState(0);
447  const ob::State *s2 = path2.getState(0);
448  const ob::State *g1 = path1.getState(path1.getStateCount() - 1);
449  const ob::State *g2 = path2.getState(path2.getStateCount() - 1);
450 
451  double regularDistance = si_->distance(s1, s2) + si_->distance(g1, g2);
452  double reversedDistance = si_->distance(s1, g2) + si_->distance(s2, g1);
453 
454  // Check if path is reversed from normal [start->goal] direction
455  if (regularDistance > reversedDistance)
456  {
457  // needs to be reversed
458  path2.reverse();
459  return true;
460  }
461 
462  return false;
463 }
const std::string getSolutionPlannerName() const
Get the best solution&#39;s planer name. Throw an exception if no solution is available.
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
Single entry for the csv data logging file.
ompl::geometric::LightningRetrieveRepair & getLightningRetrieveRepairPlanner() const
Get a pointer to the retrieve repair planner.
Definition: Lightning.h:125
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Lightning since being loaded.
Definition: Lightning.cpp:403
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:283
The planner failed to find a solution.
Definition: PlannerStatus.h:62
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
A shared pointer wrapper for ompl::base::StateSpace.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool save() override
Save the experience database to file.
Definition: Lightning.cpp:352
void clear() override
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
Definition: Lightning.cpp:141
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:274
ompl::tools::LightningDBPtr experienceDB_
A shared object between all the planners for saving and loading previous experience.
Definition: Lightning.h:210
base::State * getState(unsigned int index)
Get the state located at index along the path.
Includes various tools such as self config, benchmarking, etc.
bool reversePathIfNecessary(ompl::geometric::PathGeometric &path1, ompl::geometric::PathGeometric &path2)
If path1 and path2 have a better start/goal match when reverse, then reverse path2.
Definition: Lightning.cpp:443
ExperienceStats stats_
States data for display to console.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
ompl::tools::DynamicTimeWarpPtr dtw_
Tool for comparing two paths and scoring them.
Definition: Lightning.h:213
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool saveIfChanged() override
Save the experience database to file if there has been a change.
Definition: Lightning.cpp:362
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition: Lightning.cpp:426
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
std::string filePath_
File location of database.
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:280
void reverse()
Reverse the path.
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:277
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
void print(std::ostream &out=std::cout) const override
Print information about the current setup.
Definition: Lightning.cpp:382
A shared pointer wrapper for ompl::base::SpaceInformation.
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
Definition: Lightning.cpp:372
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: Lightning.cpp:68
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
bool scratchEnabled_
Flag indicating whether planning from scratch should be used to find solutions. Enabled by default...
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:243
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const override
Get a vector of all the planning data in the database.
Definition: Lightning.cpp:431
The exception type for ompl.
Definition: Exception.h:46
base::PlannerStatus solve(double time=1.0) override
Run the planner for up to a specified amount of time (default is 1 second)
Definition: Lightning.cpp:346
base::PlannerStatus lastStatus_
The status of the last planning request.
Definition: SimpleSetup.h:298
point now()
Get the current time point.
Definition: Time.h:70
base::PlannerDataPtr getChosenRecallPath() const
Get the chosen path used from database for repair.
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:289
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:292
base::PlannerPtr rrPlanner_
The maintained experience planner instance.
Definition: Lightning.h:204
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
Definition of a geometric path.
Definition: PathGeometric.h:60
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
bool recallEnabled_
Flag indicating whether recalled plans should be used to find solutions. Enabled by default...
std::string asString() const
Return a string representation.
void convertLogToString(const ExperienceLog &log)
Move data to string format and put in buffer.
ompl::tools::ParallelPlanPtr pp_
Instance of parallel planning to use for computing solutions in parallel.
Definition: Lightning.h:207
A shared pointer wrapper for ompl::base::PlannerData.
void convertPlannerData(const ompl::base::PlannerDataPtr &plannerData, ompl::geometric::PathGeometric &path)
Convert PlannerData to PathGeometric. Assume ordering of verticies is order of path.
Definition: Lightning.cpp:436
Lightning(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: Lightning.cpp:44
Create the set of classes typically needed to solve a geometric problem.
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68