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
40namespace og = ompl::geometric;
41namespace ob = ompl::base;
42namespace ot = ompl::tools;
43
44ompl::tools::Lightning::Lightning(const base::SpaceInformationPtr &si) : ompl::tools::ExperienceSetup(si)
45{
46 initialize();
47}
48
49ompl::tools::Lightning::Lightning(const base::StateSpacePtr &space) : ompl::tools::ExperienceSetup(space)
50{
51 initialize();
52}
53
54void 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_);
112 if (!scratchEnabled_ && !recallEnabled_)
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
164 lastStatus_ = base::PlannerStatus::UNKNOWN;
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
183 if (lastStatus_ == ompl::base::PlannerStatus::TIMEOUT)
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
258 ob::PlannerDataPtr chosenRecallPathData = getLightningRetrieveRepairPlanner().getChosenRecallPath();
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
372void 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
382void 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
403void 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
431void ompl::tools::Lightning::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
432{
433 experienceDB_->getAllPlannerDatas(plannerDatas);
434}
435
436void ompl::tools::Lightning::convertPlannerData(const ob::PlannerDataPtr &plannerData, og::PathGeometric &path)
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}
The exception type for ompl.
Definition: Exception.h:47
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
Definition: State.h:50
Definition of a geometric path.
Definition: PathGeometric.h:66
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void reverse()
Reverse the path.
base::State * getState(unsigned int index)
Get the state located at index along the path.
Create the set of classes typically needed to solve a geometric problem.
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition: Lightning.cpp:426
void clear() override
Clear all planning data. This only includes data generated by motion plan computation....
Definition: Lightning.cpp:141
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Lightning since being loaded.
Definition: Lightning.cpp:403
bool saveIfChanged() override
Save the experience database to file if there has been a change.
Definition: Lightning.cpp:362
bool save() override
Save the experience database to file.
Definition: Lightning.cpp:352
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
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
Definition: Lightning.cpp:372
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
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: Lightning.cpp:68
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
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
void print(std::ostream &out=std::cout) const override
Print information about the current setup.
Definition: Lightning.cpp:382
Lightning(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: Lightning.cpp:44
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:243
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:48
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
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
point now()
Get the current time point.
Definition: Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
Includes various tools such as self config, benchmarking, etc.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49
@ TIMEOUT
The planner failed to find a solution.
Definition: PlannerStatus.h:62
@ UNKNOWN
Uninitialized status.
Definition: PlannerStatus.h:54
Single entry for the csv data logging file.