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