Loading...
Searching...
No Matches
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
180 ExperienceLog 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{
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.
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.
void clear() override
Clear all planning data. This only includes data generated by motion plan computation....
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Lightning since being loaded.
bool saveIfChanged() override
Save the experience database to file if there has been a change.
bool save() override
Save the experience database to file.
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const override
Get a vector of all the planning data in the database.
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
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.
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.
base::PlannerStatus solve(double time=1.0) override
Run the planner for up to a specified amount of time (default is 1 second)
void print(std::ostream &out=std::cout) const override
Print information about the current setup.
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.
#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...
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
This namespace contains code that is specific to planning under geometric constraints.
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()
@ TIMEOUT
The planner failed to find a solution.
@ UNKNOWN
Uninitialized status.
Single entry for the csv data logging file.