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  recallEnabled_ = true;
57  scratchEnabled_ = true;
58 
59  // Load dynamic time warp
60  dtw_ = std::make_shared<ot::DynamicTimeWarp>(si_);
61 
62  // Load the experience database
63  experienceDB_ = std::make_shared<ompl::tools::LightningDB>(si_->getStateSpace());
64 
65  // Load the Retrieve repair database. We do it here so that setRepairPlanner() works
66  rrPlanner_ = std::make_shared<og::LightningRetrieveRepair>(si_, experienceDB_);
67 
68  OMPL_INFORM("Lightning Framework initialized.");
69 }
70 
72 {
73  if (!configured_ || !si_->isSetup() || !planner_->isSetup() || !rrPlanner_->isSetup())
74  {
75  OMPL_INFORM("Setting up the Lightning Framework");
76 
77  if (!configured_)
78  OMPL_INFORM(" Setting up because not configured");
79  else if (!si_->isSetup())
80  OMPL_INFORM(" Setting up because not si->isSetup");
81  else if (!planner_->isSetup())
82  OMPL_INFORM(" Setting up because not planner->isSetup");
83  else if (!rrPlanner_->isSetup())
84  OMPL_INFORM(" Setting up because not rrPlanner->isSetup");
85 
86  // Setup Space Information if we haven't already done so
87  if (!si_->isSetup())
88  si_->setup();
89 
90  // Setup planning from scratch planner
91  if (!planner_)
92  {
93  if (pa_)
94  planner_ = pa_(si_);
95  if (!planner_)
96  {
98  pdef_->getGoal()); // we could use the repairProblemDef_ here but that isn't setup yet
99 
100  OMPL_INFORM("No planner specified. Using default: %s", planner_->getName().c_str());
101  }
102  }
103  planner_->setProblemDefinition(pdef_);
104  if (!planner_->isSetup())
105  planner_->setup();
106 
107  // Setup planning from experience planner
108  rrPlanner_->setProblemDefinition(pdef_);
109 
110  if (!rrPlanner_->isSetup())
111  rrPlanner_->setup();
112 
113  // Create the parallel component for splitting into two threads
114  pp_ = std::make_shared<ot::ParallelPlan>(pdef_);
116  {
117  throw Exception("Both planning from scratch and experience have been disabled, unable to plan");
118  }
119  if (scratchEnabled_)
120  pp_->addPlanner(planner_); // Add the planning from scratch planner if desired
121  if (recallEnabled_)
122  pp_->addPlanner(rrPlanner_); // Add the planning from experience planner if desired
123 
124  // Check if experience database is already loaded
125  if (experienceDB_->isEmpty())
126  {
127  if (filePath_.empty())
128  {
129  OMPL_ERROR("No file path has been specified, unable to load experience DB");
130  }
131  else
132  {
133  experienceDB_->load(filePath_); // load from file
134  }
135  }
136  else
137  OMPL_ERROR("Attempting to load experience database when it is not empty");
138 
139  // Set the configured flag
140  configured_ = true;
141  }
142 }
143 
145 {
146  if (planner_)
147  planner_->clear();
148  if (rrPlanner_)
149  rrPlanner_->clear();
150  if (pdef_)
151  pdef_->clearSolutionPaths();
152  if (pp_)
153  {
154  pp_->clearHybridizationPaths();
155  }
156 }
157 
158 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner
159 // termination condition
161 {
162  OMPL_INFORM("Lightning Framework: Starting solve()");
163 
164  // Setup again in case it has not been done yet
165  setup();
166 
168  time::point start = time::now();
169 
170  // Insertion time
171  double insertionTime = 0.;
172 
173  // Start both threads
174  bool hybridize = false;
175  lastStatus_ = pp_->solve(ptc, hybridize);
176 
177  // Planning time
178  planTime_ = time::seconds(time::now() - start);
179  stats_.totalPlanningTime_ += planTime_; // used for averaging
180  stats_.numProblems_++; // used for averaging
181 
182  // Create log
184  log.planning_time = planTime_;
185 
187  {
188  // Skip further processing if absolutely no path is available
189  OMPL_ERROR("Lightning Solve: No solution found after %f seconds", planTime_);
190  stats_.numSolutionsTimedout_++;
191 
192  // Logging
193  log.planner = "neither_planner";
194  log.result = "timedout";
195  log.is_saved = "not_saved";
196  }
197  else if (!lastStatus_)
198  {
199  // Skip further processing if absolutely no path is available
200  OMPL_ERROR("Lightning Solve: Unknown failure, planner status: %s", lastStatus_.asString().c_str());
201  stats_.numSolutionsFailed_++;
202 
203  // Logging
204  log.planner = "neither_planner";
205  log.result = "failed";
206  log.is_saved = "not_saved";
207  }
208  else
209  {
210  OMPL_INFORM("Lightning Solve: Possible solution found in %f seconds", planTime_);
211 
212  // Smooth the result
213  simplifySolution(ptc);
214 
215  og::PathGeometric solutionPath = getSolutionPath(); // copied so that it is non-const
216  OMPL_INFORM("Solution path has %d states and was generated from planner %s", solutionPath.getStateCount(),
217  getSolutionPlannerName().c_str());
218 
219  // Logging
220  log.planner = getSolutionPlannerName();
221 
222  // Do not save if approximate
223  if (!haveExactSolutionPath())
224  {
225  // Logging
226  log.result = "not_exact_solution";
227  log.is_saved = "not_saved";
228  log.approximate = true;
229 
230  // Stats
231  stats_.numSolutionsApproximate_++;
232 
233  // not sure what to do here, use case not tested
234  OMPL_INFORM("NOT saving to database because the solution is APPROXIMATE");
235  }
236  // Use dynamic time warping to see if the repaired path is too similar to the original
237  else if (getSolutionPlannerName() == rrPlanner_->getName())
238  {
239  // Stats
240  stats_.numSolutionsFromRecall_++;
241 
242  // Logging
243  log.result = "from_recall";
244 
245  // Make sure solution has at least 2 states
246  if (solutionPath.getStateCount() < 2)
247  {
248  OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
249  stats_.numSolutionsTooShort_++;
250 
251  // Logging
252  log.is_saved = "less_2_states";
253  log.too_short = true;
254  }
255  else
256  {
257  // Benchmark runtime
258  time::point startTime = time::now();
259 
260  // Convert the original recalled path to PathGeometric
262  og::PathGeometric chosenRecallPath(si_);
263  convertPlannerData(chosenRecallPathData, chosenRecallPath);
264 
265  // Reverse path2 if necessary so that it matches path1 better
266  reversePathIfNecessary(solutionPath, chosenRecallPath);
267 
268  double score = dtw_->getPathsScore(solutionPath, chosenRecallPath);
269  log.score = score;
270 
271  if (score < 4)
272  {
273  OMPL_INFORM("NOT saving to database because best solution was from database and is too similar "
274  "(score %f)",
275  score);
276 
277  // Logging
278  log.insertion_failed = true;
279  log.is_saved = "score_too_similar";
280  }
281  else
282  {
283  OMPL_INFORM("Adding path to database because repaired path is different enough from original "
284  "recalled path (score %f)",
285  score);
286 
287  // Logging
288  log.insertion_failed = false;
289  log.is_saved = "score_different_enough";
290 
291  // Stats
292  stats_.numSolutionsFromRecallSaved_++;
293 
294  // Save to database
295  double dummyInsertionTime; // unused because does not include scoring function
296  experienceDB_->addPath(solutionPath, dummyInsertionTime);
297  }
298  insertionTime += time::seconds(time::now() - startTime);
299  }
300  }
301  else
302  {
303  // Logging
304  log.result = "from_scratch";
305 
306  // Stats
307  stats_.numSolutionsFromScratch_++;
308 
309  // Make sure solution has at least 2 states
310  if (solutionPath.getStateCount() < 2)
311  {
312  OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
313 
314  // Logging
315  log.is_saved = "less_2_states";
316  log.too_short = true;
317 
318  // Stats
319  stats_.numSolutionsTooShort_++;
320  }
321  else
322  {
323  OMPL_INFORM("Adding path to database because best solution was not from database");
324 
325  // Logging
326  log.result = "from_scratch";
327  log.is_saved = "saving";
328 
329  // Save to database
330  experienceDB_->addPath(solutionPath, insertionTime);
331  }
332  }
333  }
334 
335  stats_.totalInsertionTime_ += insertionTime; // used for averaging
336 
337  // Final log data
338  log.insertion_time = insertionTime;
339  log.num_vertices = experienceDB_->getStatesCount();
340  log.num_edges = 0;
341  log.num_connected_components = 0;
342 
343  // Flush the log to buffer
344  convertLogToString(log);
345 
346  return lastStatus_;
347 }
348 
350 {
351  ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(time);
352  return solve(ptc);
353 }
354 
356 {
357  if (filePath_.empty())
358  {
359  OMPL_ERROR("No file path has been specified, unable to save experience DB");
360  return false;
361  }
362  return experienceDB_->save(filePath_);
363 }
364 
366 {
367  if (filePath_.empty())
368  {
369  OMPL_ERROR("No file path has been specified, unable to save experience DB");
370  return false;
371  }
372  return experienceDB_->saveIfChanged(filePath_);
373 }
374 
375 void ompl::tools::Lightning::printResultsInfo(std::ostream &out) const
376 {
377  for (std::size_t i = 0; i < pdef_->getSolutionCount(); ++i)
378  {
379  out << "Solution " << i << " | Length: " << pdef_->getSolutions()[i].length_
380  << " | Approximate: " << (pdef_->getSolutions()[i].approximate_ ? "true" : "false")
381  << " | Planner: " << pdef_->getSolutions()[i].plannerName_ << std::endl;
382  }
383 }
384 
385 void ompl::tools::Lightning::print(std::ostream &out) const
386 {
387  if (si_)
388  {
389  si_->printProperties(out);
390  si_->printSettings(out);
391  }
392  if (planner_)
393  {
394  planner_->printProperties(out);
395  planner_->printSettings(out);
396  }
397  if (rrPlanner_)
398  {
399  rrPlanner_->printProperties(out);
400  rrPlanner_->printSettings(out);
401  }
402  if (pdef_)
403  pdef_->print(out);
404 }
405 
406 void ompl::tools::Lightning::printLogs(std::ostream &out) const
407 {
408  out << "Lightning Framework Logging Results" << std::endl;
409  out << " Solutions Attempted: " << stats_.numProblems_ << std::endl;
410  out << " Solved from scratch: " << stats_.numSolutionsFromScratch_ << " ("
411  << stats_.numSolutionsFromScratch_ / stats_.numProblems_ * 100.0 << "%)" << std::endl;
412  out << " Solved from recall: " << stats_.numSolutionsFromRecall_ << " ("
413  << stats_.numSolutionsFromRecall_ / stats_.numProblems_ * 100.0 << "%)" << std::endl;
414  out << " That were saved: " << stats_.numSolutionsFromRecallSaved_ << std::endl;
415  out << " That were discarded: " << stats_.numSolutionsFromRecall_ - stats_.numSolutionsFromRecallSaved_
416  << std::endl;
417  out << " Less than 2 states: " << stats_.numSolutionsTooShort_ << std::endl;
418  out << " Failed: " << stats_.numSolutionsFailed_ << std::endl;
419  out << " Timedout: " << stats_.numSolutionsTimedout_ << std::endl;
420  out << " Approximate: " << stats_.numSolutionsApproximate_ << std::endl;
421  out << " LightningDb " << std::endl;
422  out << " Total paths: " << experienceDB_->getExperiencesCount() << std::endl;
423  out << " Vertices (states): " << experienceDB_->getStatesCount() << std::endl;
424  out << " Unsaved solutions: " << experienceDB_->getNumUnsavedPaths() << std::endl;
425  out << " Average planning time: " << stats_.getAveragePlanningTime() << std::endl;
426  out << " Average insertion time: " << stats_.getAverageInsertionTime() << std::endl;
427 }
428 
430 {
431  return experienceDB_->getExperiencesCount();
432 }
433 
434 void ompl::tools::Lightning::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
435 {
436  experienceDB_->getAllPlannerDatas(plannerDatas);
437 }
438 
440 {
441  // Convert the planner data verticies into a vector of states
442  for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
443  path.append(plannerData->getVertex(i).getState());
444 }
445 
447 {
448  // Reverse path2 if it matches better
449  const ob::State *s1 = path1.getState(0);
450  const ob::State *s2 = path2.getState(0);
451  const ob::State *g1 = path1.getState(path1.getStateCount() - 1);
452  const ob::State *g2 = path2.getState(path2.getStateCount() - 1);
453 
454  double regularDistance = si_->distance(s1, s2) + si_->distance(g1, g2);
455  double reversedDistance = si_->distance(s1, g2) + si_->distance(s2, g1);
456 
457  // Check if path is reversed from normal [start->goal] direction
458  if (regularDistance > reversedDistance)
459  {
460  // needs to be reversed
461  path2.reverse();
462  return true;
463  }
464 
465  return false;
466 }
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:406
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:355
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:144
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:446
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:365
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition: Lightning.cpp:429
#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:385
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:375
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: Lightning.cpp:71
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:434
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:349
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:439
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