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 
44 ompl::tools::Lightning::Lightning(const base::SpaceInformationPtr &si) : ompl::tools::ExperienceSetup(si)
45 {
46  initialize();
47 }
48 
49 ompl::tools::Lightning::Lightning(const base::StateSpacePtr &space) : ompl::tools::ExperienceSetup(space)
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  SimpleSetup::setup();
73 
74  // Setup planning from experience planner
75  rrPlanner_->setProblemDefinition(pdef_);
76 
77  if (!rrPlanner_->isSetup())
78  rrPlanner_->setup();
79 
80  // Create the parallel component for splitting into two threads
81  pp_ = std::make_shared<ot::ParallelPlan>(pdef_);
82  if (!scratchEnabled_ && !recallEnabled_)
83  {
84  throw Exception("Both planning from scratch and experience have been disabled, unable to plan");
85  }
86  if (scratchEnabled_)
87  pp_->addPlanner(planner_); // Add the planning from scratch planner if desired
88  if (recallEnabled_)
89  pp_->addPlanner(rrPlanner_); // Add the planning from experience planner if desired
90 
91  // Check if experience database is already loaded
92  if (experienceDB_->isEmpty())
93  {
94  if (filePath_.empty())
95  {
96  OMPL_ERROR("No file path has been specified, unable to load experience DB");
97  return;
98  }
99  else
100  {
101  experienceDB_->load(filePath_); // load from file
102  }
103  }
104  else
105  OMPL_ERROR("Attempting to load experience database when it is not empty");
106  }
107 }
108 
110 {
111  if (planner_)
112  planner_->clear();
113  if (rrPlanner_)
114  rrPlanner_->clear();
115  if (pdef_)
116  pdef_->clearSolutionPaths();
117  if (pp_)
118  {
119  pp_->clearHybridizationPaths();
120  }
121 }
122 
123 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner
124 // termination condition
126 {
127  OMPL_INFORM("Lightning Framework: Starting solve()");
128 
129  // Setup again in case it has not been done yet
130  setup();
131 
132  lastStatus_ = base::PlannerStatus::UNKNOWN;
133  time::point start = time::now();
134 
135  // Insertion time
136  double insertionTime = 0.;
137 
138  // Start both threads
139  bool hybridize = false;
140  lastStatus_ = pp_->solve(ptc, hybridize);
141 
142  // Planning time
143  planTime_ = time::seconds(time::now() - start);
144  stats_.totalPlanningTime_ += planTime_; // used for averaging
145  stats_.numProblems_++; // used for averaging
146 
147  // Create log
149  log.planning_time = planTime_;
150 
151  if (lastStatus_ == ompl::base::PlannerStatus::TIMEOUT)
152  {
153  // Skip further processing if absolutely no path is available
154  OMPL_ERROR("Lightning Solve: No solution found after %f seconds", planTime_);
155  stats_.numSolutionsTimedout_++;
156 
157  // Logging
158  log.planner = "neither_planner";
159  log.result = "timedout";
160  log.is_saved = "not_saved";
161  }
162  else if (!lastStatus_)
163  {
164  // Skip further processing if absolutely no path is available
165  OMPL_ERROR("Lightning Solve: Unknown failure, planner status: %s", lastStatus_.asString().c_str());
166  stats_.numSolutionsFailed_++;
167 
168  // Logging
169  log.planner = "neither_planner";
170  log.result = "failed";
171  log.is_saved = "not_saved";
172  }
173  else
174  {
175  OMPL_INFORM("Lightning Solve: Possible solution found in %f seconds", planTime_);
176 
177  // Smooth the result
178  simplifySolution(ptc);
179 
180  og::PathGeometric solutionPath = getSolutionPath(); // copied so that it is non-const
181  OMPL_INFORM("Solution path has %d states and was generated from planner %s", solutionPath.getStateCount(),
182  getSolutionPlannerName().c_str());
183 
184  // Logging
185  log.planner = getSolutionPlannerName();
186 
187  // Do not save if approximate
188  if (!haveExactSolutionPath())
189  {
190  // Logging
191  log.result = "not_exact_solution";
192  log.is_saved = "not_saved";
193  log.approximate = true;
194 
195  // Stats
196  stats_.numSolutionsApproximate_++;
197 
198  // not sure what to do here, use case not tested
199  OMPL_INFORM("NOT saving to database because the solution is APPROXIMATE");
200  }
201  // Use dynamic time warping to see if the repaired path is too similar to the original
202  else if (getSolutionPlannerName() == rrPlanner_->getName())
203  {
204  // Stats
205  stats_.numSolutionsFromRecall_++;
206 
207  // Logging
208  log.result = "from_recall";
209 
210  // Make sure solution has at least 2 states
211  if (solutionPath.getStateCount() < 2)
212  {
213  OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
214  stats_.numSolutionsTooShort_++;
215 
216  // Logging
217  log.is_saved = "less_2_states";
218  log.too_short = true;
219  }
220  else
221  {
222  // Benchmark runtime
223  time::point startTime = time::now();
224 
225  // Convert the original recalled path to PathGeometric
226  ob::PlannerDataPtr chosenRecallPathData = getLightningRetrieveRepairPlanner().getChosenRecallPath();
227  og::PathGeometric chosenRecallPath(si_);
228  convertPlannerData(chosenRecallPathData, chosenRecallPath);
229 
230  // Reverse path2 if necessary so that it matches path1 better
231  reversePathIfNecessary(solutionPath, chosenRecallPath);
232 
233  double score = dtw_->getPathsScore(solutionPath, chosenRecallPath);
234  log.score = score;
235 
236  if (score < 4)
237  {
238  OMPL_INFORM("NOT saving to database because best solution was from database and is too similar "
239  "(score %f)",
240  score);
241 
242  // Logging
243  log.insertion_failed = true;
244  log.is_saved = "score_too_similar";
245  }
246  else
247  {
248  OMPL_INFORM("Adding path to database because repaired path is different enough from original "
249  "recalled path (score %f)",
250  score);
251 
252  // Logging
253  log.insertion_failed = false;
254  log.is_saved = "score_different_enough";
255 
256  // Stats
257  stats_.numSolutionsFromRecallSaved_++;
258 
259  // Save to database
260  double dummyInsertionTime; // unused because does not include scoring function
261  experienceDB_->addPath(solutionPath, dummyInsertionTime);
262  }
263  insertionTime += time::seconds(time::now() - startTime);
264  }
265  }
266  else
267  {
268  // Logging
269  log.result = "from_scratch";
270 
271  // Stats
272  stats_.numSolutionsFromScratch_++;
273 
274  // Make sure solution has at least 2 states
275  if (solutionPath.getStateCount() < 2)
276  {
277  OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
278 
279  // Logging
280  log.is_saved = "less_2_states";
281  log.too_short = true;
282 
283  // Stats
284  stats_.numSolutionsTooShort_++;
285  }
286  else
287  {
288  OMPL_INFORM("Adding path to database because best solution was not from database");
289 
290  // Logging
291  log.result = "from_scratch";
292  log.is_saved = "saving";
293 
294  // Save to database
295  experienceDB_->addPath(solutionPath, insertionTime);
296  }
297  }
298  }
299 
300  stats_.totalInsertionTime_ += insertionTime; // used for averaging
301 
302  // Final log data
303  log.insertion_time = insertionTime;
304  log.num_vertices = experienceDB_->getStatesCount();
305  log.num_edges = 0;
306  log.num_connected_components = 0;
307 
308  // Flush the log to buffer
309  convertLogToString(log);
310 
311  return lastStatus_;
312 }
313 
315 {
316  ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(time);
317  return solve(ptc);
318 }
319 
321 {
322  if (filePath_.empty())
323  {
324  OMPL_ERROR("No file path has been specified, unable to save experience DB");
325  return false;
326  }
327  return experienceDB_->save(filePath_);
328 }
329 
331 {
332  if (filePath_.empty())
333  {
334  OMPL_ERROR("No file path has been specified, unable to save experience DB");
335  return false;
336  }
337  return experienceDB_->saveIfChanged(filePath_);
338 }
339 
340 void ompl::tools::Lightning::printResultsInfo(std::ostream &out) const
341 {
342  for (std::size_t i = 0; i < pdef_->getSolutionCount(); ++i)
343  {
344  out << "Solution " << i << " | Length: " << pdef_->getSolutions()[i].length_
345  << " | Approximate: " << (pdef_->getSolutions()[i].approximate_ ? "true" : "false")
346  << " | Planner: " << pdef_->getSolutions()[i].plannerName_ << std::endl;
347  }
348 }
349 
350 void ompl::tools::Lightning::print(std::ostream &out) const
351 {
352  if (si_)
353  {
354  si_->printProperties(out);
355  si_->printSettings(out);
356  }
357  if (planner_)
358  {
359  planner_->printProperties(out);
360  planner_->printSettings(out);
361  }
362  if (rrPlanner_)
363  {
364  rrPlanner_->printProperties(out);
365  rrPlanner_->printSettings(out);
366  }
367  if (pdef_)
368  pdef_->print(out);
369 }
370 
371 void ompl::tools::Lightning::printLogs(std::ostream &out) const
372 {
373  out << "Lightning Framework Logging Results" << std::endl;
374  out << " Solutions Attempted: " << stats_.numProblems_ << std::endl;
375  out << " Solved from scratch: " << stats_.numSolutionsFromScratch_ << " ("
376  << stats_.numSolutionsFromScratch_ / stats_.numProblems_ * 100.0 << "%)" << std::endl;
377  out << " Solved from recall: " << stats_.numSolutionsFromRecall_ << " ("
378  << stats_.numSolutionsFromRecall_ / stats_.numProblems_ * 100.0 << "%)" << std::endl;
379  out << " That were saved: " << stats_.numSolutionsFromRecallSaved_ << std::endl;
380  out << " That were discarded: " << stats_.numSolutionsFromRecall_ - stats_.numSolutionsFromRecallSaved_
381  << std::endl;
382  out << " Less than 2 states: " << stats_.numSolutionsTooShort_ << std::endl;
383  out << " Failed: " << stats_.numSolutionsFailed_ << std::endl;
384  out << " Timedout: " << stats_.numSolutionsTimedout_ << std::endl;
385  out << " Approximate: " << stats_.numSolutionsApproximate_ << std::endl;
386  out << " LightningDb " << std::endl;
387  out << " Total paths: " << experienceDB_->getExperiencesCount() << std::endl;
388  out << " Vertices (states): " << experienceDB_->getStatesCount() << std::endl;
389  out << " Unsaved solutions: " << experienceDB_->getNumUnsavedPaths() << std::endl;
390  out << " Average planning time: " << stats_.getAveragePlanningTime() << std::endl;
391  out << " Average insertion time: " << stats_.getAverageInsertionTime() << std::endl;
392 }
393 
395 {
396  return experienceDB_->getExperiencesCount();
397 }
398 
399 void ompl::tools::Lightning::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
400 {
401  experienceDB_->getAllPlannerDatas(plannerDatas);
402 }
403 
404 void ompl::tools::Lightning::convertPlannerData(const ob::PlannerDataPtr &plannerData, og::PathGeometric &path)
405 {
406  // Convert the planner data vertices into a vector of states
407  for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
408  path.append(plannerData->getVertex(i).getState());
409 }
410 
412 {
413  // Reverse path2 if it matches better
414  const ob::State *s1 = path1.getState(0);
415  const ob::State *s2 = path2.getState(0);
416  const ob::State *g1 = path1.getState(path1.getStateCount() - 1);
417  const ob::State *g2 = path2.getState(path2.getStateCount() - 1);
418 
419  double regularDistance = si_->distance(s1, s2) + si_->distance(g1, g2);
420  double reversedDistance = si_->distance(s1, g2) + si_->distance(s2, g1);
421 
422  // Check if path is reversed from normal [start->goal] direction
423  if (regularDistance > reversedDistance)
424  {
425  // needs to be reversed
426  path2.reverse();
427  return true;
428  }
429 
430  return false;
431 }
void reverse()
Reverse the path.
Lightning(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: Lightning.cpp:44
Includes various tools such as self config, benchmarking, etc.
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Lightning since being loaded.
Definition: Lightning.cpp:371
bool saveIfChanged() override
Save the experience database to file if there has been a change.
Definition: Lightning.cpp:330
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:79
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition: Lightning.cpp:394
void convertPlannerData(const ompl::base::PlannerDataPtr &plannerData, ompl::geometric::PathGeometric &path)
Convert PlannerData to PathGeometric. Assume ordering of vertices is order of path.
Definition: Lightning.cpp:404
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: Lightning.cpp:68
point now()
Get the current time point.
Definition: Time.h:122
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
@ TIMEOUT
The planner failed to find a solution.
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const override
Get a vector of all the planning data in the database.
Definition: Lightning.cpp:399
void clear() override
Clear all planning data. This only includes data generated by motion plan computation....
Definition: Lightning.cpp:109
Definition of a geometric path.
Definition: PathGeometric.h:97
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
Definition: Lightning.cpp:340
bool save() override
Save the experience database to file.
Definition: Lightning.cpp:320
A class to store the exit status of Planner::solve()
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void print(std::ostream &out=std::cout) const override
Print information about the current setup.
Definition: Lightning.cpp:350
Single entry for the csv data logging file.
Create the set of classes typically needed to solve a geometric problem.
@ UNKNOWN
Uninitialized status.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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:314
The exception type for ompl.
Definition: Exception.h:78
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:411
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
base::State * getState(unsigned int index)
Get the state located at index along the path.
Main namespace. Contains everything in this library.