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 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
148 ExperienceLog 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
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
340void 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
350void 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
371void 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
399void ompl::tools::Lightning::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
400{
401 experienceDB_->getAllPlannerDatas(plannerDatas);
402}
403
404void 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}
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 vertices 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
#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.