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_);
83 {
84 throw Exception("Both planning from scratch and experience have been disabled, unable to plan");
85 }
87 pp_->addPlanner(planner_); // Add the planning from scratch planner if desired
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
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
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
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
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.
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
double planTime_
The amount of time the last planning step took.
base::PlannerStatus lastStatus_
The status of the last planning request.
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
const std::string getSolutionPlannerName() const
Get the best solution's planer name. Throw an exception if no solution is available.
bool configured_
Flag indicating whether the classes needed for planning are set up.
base::PlannerPtr planner_
The maintained planner instance.
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
base::SpaceInformationPtr si_
The created space information.
base::ProblemDefinitionPtr pdef_
The created problem definition.
std::string filePath_
File location of database.
void convertLogToString(const ExperienceLog &log)
Move data to string format and put in buffer.
ExperienceStats stats_
States data for display to console.
bool recallEnabled_
Flag indicating whether recalled plans should be used to find solutions. Enabled by default.
bool scratchEnabled_
Flag indicating whether planning from scratch should be used to find solutions. Enabled by default.
ExperienceSetup(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
ompl::tools::LightningDBPtr experienceDB_
A shared object between all the planners for saving and loading previous experience.
Definition Lightning.h:210
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
ompl::geometric::LightningRetrieveRepair & getLightningRetrieveRepairPlanner() const
Get a pointer to the retrieve repair planner.
Definition Lightning.h:125
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.
ompl::tools::ParallelPlanPtr pp_
Instance of parallel planning to use for computing solutions in parallel.
Definition Lightning.h:207
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
base::PlannerPtr rrPlanner_
The maintained experience planner instance.
Definition Lightning.h:204
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
ompl::tools::DynamicTimeWarpPtr dtw_
Tool for comparing two paths and scoring them.
Definition Lightning.h:213
#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.
Namespace containing time datatypes and time operations.
Definition Time.h:50
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.