Thunder.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/thunder/Thunder.h>
38#include <ompl/geometric/planners/rrt/RRTConnect.h>
39#include <ompl/base/PlannerStatus.h>
40#include <ompl/util/Console.h>
41
42namespace og = ompl::geometric;
43namespace ob = ompl::base;
44namespace ot = ompl::tools;
45
46ompl::tools::Thunder::Thunder(const base::SpaceInformationPtr &si) : ompl::tools::ExperienceSetup(si)
47{
48 initialize();
49}
50
51ompl::tools::Thunder::Thunder(const base::StateSpacePtr &space) : ompl::tools::ExperienceSetup(space)
52{
53 initialize();
54}
55
56void ompl::tools::Thunder::initialize()
57{
58 OMPL_INFORM("Initializing Thunder Framework");
59
60 filePath_ = "unloaded";
61
62 // Load the experience database
63 experienceDB_ = std::make_shared<ompl::tools::ThunderDB>(si_->getStateSpace());
64
65 // Load the Retrieve repair database. We do it here so that setRepairPlanner() works
66 rrPlanner_ = std::make_shared<og::ThunderRetrieveRepair>(si_, experienceDB_);
67
68 OMPL_INFORM("Thunder Framework initialized.");
69}
70
72{
73 if (!configured_ || !si_->isSetup() || !planner_->isSetup() || !rrPlanner_->isSetup())
74 {
75 // Setup Space Information if we haven't already done so
76 if (!si_->isSetup())
77 si_->setup();
78
79 // Setup planning from scratch planner
80 if (!planner_)
81 {
82 if (pa_)
83 planner_ = pa_(si_);
84 if (!planner_)
85 {
86 OMPL_INFORM("Getting default planner: ");
87 planner_ = std::make_shared<ompl::geometric::RRTConnect>(si_);
88 // This was disabled because i like to use Thunder / SPARSdb without setting a goal definition
89 // planner_ = ompl::geometric::getDefaultPlanner(pdef_->getGoal()); // we could use the
90 // repairProblemDef_ here but that isn't setup yet
91
92 OMPL_INFORM("No planner specified. Using default: %s", planner_->getName().c_str());
93 }
94 }
95 planner_->setProblemDefinition(pdef_);
96 if (!planner_->isSetup())
97 planner_->setup();
98
99 // Decide if we should setup the second planning from scratch planner for benchmarking w/o recall
100 if (dualThreadScratchEnabled_ && !recallEnabled_)
101 {
102 // Setup planning from scratch planner 2
103 if (!planner2_)
104 {
105 if (pa_)
106 planner2_ = pa_(si_);
107 if (!planner2_)
108 {
109 OMPL_INFORM("Getting default planner: ");
110 planner2_ = std::make_shared<ompl::geometric::RRTConnect>(si_);
111 // This was disabled because i like to use Thunder / SPARSdb without setting a goal definition
112 // planner2_ = ompl::geometric::getDefaultPlanner(pdef_->getGoal()); // we could use the
113 // repairProblemDef_ here but that isn't setup yet
114
115 OMPL_INFORM("No planner 2 specified. Using default: %s", planner2_->getName().c_str());
116 }
117 }
118 planner2_->setProblemDefinition(pdef_);
119 if (!planner2_->isSetup())
120 planner2_->setup();
121 }
122
123 // Setup planning from experience planner
124 rrPlanner_->setProblemDefinition(pdef_);
125
126 if (!rrPlanner_->isSetup())
127 rrPlanner_->setup();
128
129 // Create the parallel component for splitting into two threads
130 pp_ = std::make_shared<ot::ParallelPlan>(pdef_);
131 if (!scratchEnabled_ && !recallEnabled_)
132 {
133 throw Exception("Both planning from scratch and experience have been disabled, unable to plan");
134 }
135 if (recallEnabled_)
136 pp_->addPlanner(rrPlanner_); // Add the planning from experience planner if desired
137 if (scratchEnabled_)
138 pp_->addPlanner(planner_); // Add the planning from scratch planner if desired
139 if (dualThreadScratchEnabled_ && !recallEnabled_)
140 {
141 OMPL_INFORM("Adding second planning from scratch planner");
142 pp_->addPlanner(planner2_); // Add a SECOND planning from scratch planner if desired
143 }
144
145 // Setup SPARS
146 if (!experienceDB_->getSPARSdb())
147 {
148 OMPL_INFORM("Calling setup() for SPARSdb");
149
150 // Load SPARSdb
151 experienceDB_->getSPARSdb() = std::make_shared<ompl::geometric::SPARSdb>(si_);
152 experienceDB_->getSPARSdb()->setProblemDefinition(pdef_);
153 experienceDB_->getSPARSdb()->setup();
154
155 experienceDB_->getSPARSdb()->setStretchFactor(1.2);
156 experienceDB_->getSPARSdb()->setSparseDeltaFraction(
157 0.05); // vertex visibility range = maximum_extent * this_fraction
158 // experienceDB_->getSPARSdb()->setDenseDeltaFraction(0.001);
159
160 experienceDB_->getSPARSdb()->printDebug();
161
162 experienceDB_->load(filePath_); // load from file
163 }
164
165 // Set the configured flag
166 configured_ = true;
167 }
168}
169
171{
172 if (planner_)
173 planner_->clear();
174 if (rrPlanner_)
175 rrPlanner_->clear();
176 if (planner2_)
177 planner2_->clear();
178 if (pdef_)
179 pdef_->clearSolutionPaths();
180 if (pp_)
181 {
182 pp_->clearHybridizationPaths();
183 }
184}
185
187{
188 pa_ = pa;
189 planner_.reset();
190 // note: the rrPlanner_ never uses the allocator so does not need to be reset
191 configured_ = false;
192}
193
195{
196 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner
197 // termination condition
198
199 OMPL_INFORM("Thunder Framework: Starting solve()");
200
201 // Setup again in case it has not been done yet
202 setup();
203
204 lastStatus_ = base::PlannerStatus::UNKNOWN;
205 time::point start = time::now();
206
207 // Warn if there are queued paths that have not been added to the experience database
208 if (!queuedSolutionPaths_.empty())
209 {
210 OMPL_WARN("Previous solved paths are currently uninserted into the experience database and are in the "
211 "post-proccessing queue");
212 }
213
214 // There are two modes for running parallel plan - one in which both threads are run until they both return a result
215 // and/or fail
216 // The second mode stops with the first solution found - we want this one
217 bool stopWhenFirstSolutionFound = true;
218
219 if (stopWhenFirstSolutionFound)
220 {
221 // If \e hybridize is false, when the first solution is found, the rest of the planners are stopped as well.
222 // OMPL_DEBUG("Thunder: stopping when first solution is found from either thread");
223 // Start both threads
224 bool hybridize = false;
225 lastStatus_ = pp_->solve(ptc, hybridize);
226 }
227 else
228 {
229 OMPL_WARN("Thunder: not stopping until a solution or a failure is found from both threads. THIS MODE IS JUST "
230 "FOR TESTING");
231 // This mode is more for benchmarking, since I don't care about optimality
232 // If \e hybridize is false, when \e minSolCount new solutions are found (added to the set of solutions
233 // maintained by ompl::base::Goal), the rest of the planners are stopped as well.
234
235 // Start both threads
236 std::size_t minSolCount = 2;
237 std::size_t maxSolCount = 2;
238 bool hybridize = false;
239 lastStatus_ = pp_->solve(ptc, minSolCount, maxSolCount, hybridize);
240 }
241
242 // Planning time
243 planTime_ = time::seconds(time::now() - start);
244
245 // Create log
247 log.planning_time = planTime_;
248
249 // Record stats
250 stats_.totalPlanningTime_ += planTime_; // used for averaging
251 stats_.numProblems_++; // used for averaging
252
253 if (lastStatus_ == ompl::base::PlannerStatus::TIMEOUT)
254 {
255 // Skip further processing if absolutely no path is available
256 OMPL_ERROR("Thunder Solve: No solution found after %f seconds", planTime_);
257
258 stats_.numSolutionsTimedout_++;
259
260 // Logging
261 log.planner = "neither_planner";
262 log.result = "timedout";
263 log.is_saved = "not_saved";
264 }
265 else if (!lastStatus_)
266 {
267 // Skip further processing if absolutely no path is available
268 OMPL_ERROR("Thunder Solve: Unknown failure");
269 stats_.numSolutionsFailed_++;
270
271 // Logging
272 log.planner = "neither_planner";
273 log.result = "failed";
274 log.is_saved = "not_saved";
275 }
276 else
277 {
278 OMPL_INFORM("Thunder Solve: Possible solution found in %f seconds", planTime_);
279
280 // Smooth the result
281 simplifySolution(ptc);
282
283 og::PathGeometric solutionPath = getSolutionPath(); // copied so that it is non-const
284 OMPL_INFORM("Solution path has %d states and was generated from planner %s", solutionPath.getStateCount(),
285 getSolutionPlannerName().c_str());
286
287 // Logging
288 log.planner = getSolutionPlannerName();
289
290 // Do not save if approximate
291 if (!haveExactSolutionPath())
292 {
293 OMPL_INFORM("THUNDER RESULTS: Approximate");
294
295 // Logging
296 log.result = "not_exact_solution";
297 log.is_saved = "not_saved";
298 log.approximate = true;
299
300 // Stats
301 stats_.numSolutionsApproximate_++;
302
303 // TODO not sure what to do here, use case not tested
304 OMPL_WARN("NOT saving to database because the solution is APPROXIMATE");
305 }
306 else if (getSolutionPlannerName() == rrPlanner_->getName())
307 {
308 OMPL_INFORM("THUNDER RESULTS: From Recall");
309
310 // Stats
311 stats_.numSolutionsFromRecall_++;
312
313 // Logging
314 log.result = "from_recall";
315
316 // Make sure solution has at least 2 states
317 if (solutionPath.getStateCount() < 2)
318 {
319 OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
320 stats_.numSolutionsTooShort_++;
321
322 // Logging
323 log.is_saved = "less_2_states";
324 log.too_short = true;
325 }
326 else if (false) // always add when from recall
327 {
328 OMPL_INFORM("Adding path to database because SPARS will decide for us if we should keep the nodes");
329
330 // Stats
331 stats_.numSolutionsFromRecallSaved_++;
332
333 // Queue the solution path for future insertion into experience database (post-processing)
334 queuedSolutionPaths_.push_back(solutionPath);
335
336 // Logging
337 log.insertion_failed = false; // TODO this is wrong logging data
338 log.is_saved = "always_attempt";
339 }
340 else // never add when from recall
341 {
342 OMPL_INFORM("NOT adding path to database because SPARS already has it");
343
344 // Logging
345 log.is_saved = "skipped";
346 }
347 }
348 else
349 {
350 OMPL_INFORM("THUNDER RESULTS: From Scratch");
351
352 // Logging
353 log.result = "from_scratch";
354
355 // Stats
356 stats_.numSolutionsFromScratch_++;
357
358 // Make sure solution has at least 2 states
359 if (solutionPath.getStateCount() < 2)
360 {
361 OMPL_INFORM("NOT saving to database because solution is less than 2 states long");
362
363 // Logging
364 log.is_saved = "less_2_states";
365 log.too_short = true;
366
367 // Stats
368 stats_.numSolutionsTooShort_++;
369 }
370 else
371 {
372 OMPL_INFORM("Adding path to database because best solution was not from database");
373
374 // Logging
375 log.result = "from_scratch";
376 log.is_saved = "saving";
377
378 // Queue the solution path for future insertion into experience database (post-processing)
379 queuedSolutionPaths_.push_back(solutionPath);
380
381 log.insertion_failed = false; // TODO fix this wrong logging info
382 }
383 }
384 }
385
386 // Final log data
387 // log.insertion_time = insertionTime; TODO fix this
388 log.num_vertices = experienceDB_->getSPARSdb()->getNumVertices();
389 log.num_edges = experienceDB_->getSPARSdb()->getNumEdges();
390 log.num_connected_components = experienceDB_->getSPARSdb()->getNumConnectedComponents();
391
392 // Flush the log to buffer
393 convertLogToString(log);
394
395 return lastStatus_;
396}
397
399{
400 ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(time);
401 return solve(ptc);
402}
403
405{
406 setup(); // ensure the PRM db has been loaded to the Experience DB
407 return experienceDB_->save(filePath_);
408}
409
411{
412 setup(); // ensure the PRM db has been loaded to the Experience DB
413 return experienceDB_->saveIfChanged(filePath_);
414}
415
416void ompl::tools::Thunder::printResultsInfo(std::ostream &out) const
417{
418 for (std::size_t i = 0; i < pdef_->getSolutionCount(); ++i)
419 {
420 out << "Solution " << i << "\t | Length: " << pdef_->getSolutions()[i].length_
421 << "\t | Approximate: " << (pdef_->getSolutions()[i].approximate_ ? "true" : "false")
422 << "\t | Planner: " << pdef_->getSolutions()[i].plannerName_ << std::endl;
423 }
424}
425
426void ompl::tools::Thunder::print(std::ostream &out) const
427{
428 if (si_)
429 {
430 si_->printProperties(out);
431 si_->printSettings(out);
432 }
433 if (planner_)
434 {
435 planner_->printProperties(out);
436 planner_->printSettings(out);
437 }
438 if (rrPlanner_)
439 {
440 rrPlanner_->printProperties(out);
441 rrPlanner_->printSettings(out);
442 }
443 if (planner2_)
444 {
445 planner2_->printProperties(out);
446 planner2_->printSettings(out);
447 }
448 if (pdef_)
449 pdef_->print(out);
450}
451
452void ompl::tools::Thunder::printLogs(std::ostream &out) const
453{
454 if (!recallEnabled_)
455 out << "Scratch Planning Logging Results (inside Thunder Framework)" << std::endl;
456 else
457 out << "Thunder Framework Logging Results" << std::endl;
458 out << " Solutions Attempted: " << stats_.numProblems_ << std::endl;
459 out << " Solved from scratch: " << stats_.numSolutionsFromScratch_ << " ("
460 << stats_.numSolutionsFromScratch_ / stats_.numProblems_ * 100 << "%)" << std::endl;
461 out << " Solved from recall: " << stats_.numSolutionsFromRecall_ << " ("
462 << stats_.numSolutionsFromRecall_ / stats_.numProblems_ * 100 << "%)" << std::endl;
463 out << " That were saved: " << stats_.numSolutionsFromRecallSaved_ << std::endl;
464 out << " That were discarded: " << stats_.numSolutionsFromRecall_ - stats_.numSolutionsFromRecallSaved_
465 << std::endl;
466 out << " Less than 2 states: " << stats_.numSolutionsTooShort_ << std::endl;
467 out << " Failed: " << stats_.numSolutionsFailed_ << std::endl;
468 out << " Timedout: " << stats_.numSolutionsTimedout_ << std::endl;
469 out << " Approximate: " << stats_.numSolutionsApproximate_ << std::endl;
470 out << " SPARSdb " << std::endl;
471 out << " Vertices: " << experienceDB_->getSPARSdb()->getNumVertices() << std::endl;
472 out << " Edges: " << experienceDB_->getSPARSdb()->getNumEdges() << std::endl;
473 out << " Connected Components: " << experienceDB_->getSPARSdb()->getNumConnectedComponents() << std::endl;
474 out << " Unsaved paths inserted: " << experienceDB_->getNumPathsInserted() << std::endl;
475 out << " Consecutive state failures: " << experienceDB_->getSPARSdb()->getNumConsecutiveFailures() << std::endl;
476 out << " Connected path failures: " << experienceDB_->getSPARSdb()->getNumPathInsertionFailed() << std::endl;
477 out << " Sparse Delta Fraction: " << experienceDB_->getSPARSdb()->getSparseDeltaFraction() << std::endl;
478 out << " Average planning time: " << stats_.getAveragePlanningTime() << std::endl;
479 out << " Average insertion time: " << stats_.getAverageInsertionTime() << std::endl;
480}
481
483{
484 return experienceDB_->getSPARSdb()->getNumVertices();
485}
486
487void ompl::tools::Thunder::getAllPlannerDatas(std::vector<ob::PlannerDataPtr> &plannerDatas) const
488{
489 experienceDB_->getAllPlannerDatas(plannerDatas);
490}
491
492void ompl::tools::Thunder::convertPlannerData(const ob::PlannerDataPtr &plannerData, og::PathGeometric &path)
493{
494 // Convert the planner data verticies into a vector of states
495 for (std::size_t i = 0; i < plannerData->numVertices(); ++i)
496 path.append(plannerData->getVertex(i).getState());
497}
498
500{
501 // Reverse path2 if it matches better
502 const ob::State *s1 = path1.getState(0);
503 const ob::State *s2 = path2.getState(0);
504 const ob::State *g1 = path1.getState(path1.getStateCount() - 1);
505 const ob::State *g2 = path2.getState(path2.getStateCount() - 1);
506
507 double regularDistance = si_->distance(s1, s2) + si_->distance(g1, g2);
508 double reversedDistance = si_->distance(s1, g2) + si_->distance(s2, g1);
509
510 // Check if path is reversed from normal [start->goal] direction
511 if (regularDistance > reversedDistance)
512 {
513 // needs to be reversed
514 path2.reverse();
515 return true;
516 }
517
518 return false;
519}
520
521ompl::tools::ThunderDBPtr ompl::tools::Thunder::getExperienceDB()
522{
523 return experienceDB_;
524}
525
527{
528 OMPL_INFORM("Performing post-processing");
529
530 for (auto &queuedSolutionPath : queuedSolutionPaths_)
531 {
532 // Time to add a path to experience database
533 double insertionTime;
534
535 experienceDB_->addPath(queuedSolutionPath, insertionTime);
536 OMPL_INFORM("Finished inserting experience path in %f seconds", insertionTime);
537 stats_.totalInsertionTime_ += insertionTime; // used for averaging
538 }
539
540 // Remove all inserted paths from the queue
541 queuedSolutionPaths_.clear();
542
543 return true;
544}
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.
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: Thunder.cpp:499
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
Definition: Thunder.cpp:416
bool doPostProcessing() override
Allow accumlated experiences to be processed.
Definition: Thunder.cpp:526
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Thunder since being loaded.
Definition: Thunder.cpp:452
base::PlannerStatus solve(double time=1.0) override
Run the planner for up to a specified amount of time (default is 1 second)
Definition: Thunder.cpp:398
void print(std::ostream &out=std::cout) const override
Print information about the current setup.
Definition: Thunder.cpp:426
Thunder(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: Thunder.cpp:46
bool save() override
Save the experience database to file.
Definition: Thunder.cpp:404
void clear() override
Clear all planning data. This only includes data generated by motion plan computation....
Definition: Thunder.cpp:170
bool saveIfChanged() override
Save the experience database to file if there has been a change.
Definition: Thunder.cpp:410
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition: Thunder.cpp:482
ompl::tools::ThunderDBPtr getExperienceDB()
Hook for getting access to debug data.
Definition: Thunder.cpp:521
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: Thunder.cpp:71
void setPlannerAllocator(const base::PlannerAllocator &pa)
Set the planner allocator to use. This is only used if no planner has been set. This is optional – a ...
Definition: Thunder.cpp:186
void convertPlannerData(const ompl::base::PlannerDataPtr &plannerData, ompl::geometric::PathGeometric &path)
Convert PlannerData to PathGeometric. Assume ordering of verticies is order of path.
Definition: Thunder.cpp:492
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const override
Get a vector of all the planning data in the database.
Definition: Thunder.cpp:487
#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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:444
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.