Lightning.h
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  Desc: Implementation of the Lightning Framework for experienced-based planning
37 
38  Paper: Berenson, Dmitry, Pieter Abbeel, and Ken Goldberg.
39  "A robot path planning framework that learns from experience."
40  Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012.
41 
42  Notes: The user of this class should invoke the loading and saving from file, otherwise experiences
43  will be lost.
44 */
45 
46 #ifndef OMPL_TOOLS_LIGHTNING_LIGHTNING_
47 #define OMPL_TOOLS_LIGHTNING_LIGHTNING_
48 
49 #include "ompl/tools/experience/ExperienceSetup.h"
50 #include "ompl/base/Planner.h"
51 #include "ompl/base/PlannerData.h"
52 #include "ompl/base/ProblemDefinition.h"
53 #include "ompl/base/SpaceInformation.h"
54 #include "ompl/base/ProblemDefinition.h"
55 
56 #include "ompl/geometric/PathGeometric.h"
57 #include "ompl/geometric/PathSimplifier.h"
58 #include "ompl/geometric/planners/experience/LightningRetrieveRepair.h"
59 
60 #include "ompl/tools/multiplan/ParallelPlan.h"
61 #include "ompl/tools/config/SelfConfig.h"
62 
63 #include "ompl/util/Console.h"
64 #include "ompl/util/Exception.h"
65 
66 #include "ompl/tools/lightning/DynamicTimeWarp.h"
67 
68 namespace ompl
69 {
70  namespace tools
71  {
72  // class LightningDB; // forward declaration
73  OMPL_CLASS_FORWARD(LightningDB);
74  OMPL_CLASS_FORWARD(ParallelPlan);
75 
90  OMPL_CLASS_FORWARD(Lightning);
93 
99  {
100  public:
102  explicit Lightning(const base::SpaceInformationPtr &si);
103 
107  explicit Lightning(const base::StateSpacePtr &space);
108 
109  private:
113  void initialize();
114 
115  public:
117  void printResultsInfo(std::ostream &out = std::cout) const override;
118 
120  void printLogs(std::ostream &out = std::cout) const override;
121 
126  {
127  return static_cast<ompl::geometric::LightningRetrieveRepair &>(*rrPlanner_);
128  }
129 
133  void setRepairPlanner(const base::PlannerPtr &planner) override
134  {
135  static_cast<og::LightningRetrieveRepair &>(*rrPlanner_).setRepairPlanner(planner);
136  }
137 
142  {
143  pa_ = pa;
144  planner_.reset();
145  // note: the rrPlanner_ never uses the allocator so does not need to be reset
146  configured_ = false;
147  }
148 
150  base::PlannerStatus solve(double time = 1.0) override;
151 
154 
156  bool save() override;
157 
159  bool saveIfChanged() override;
160 
164  void clear() override;
165 
167  void print(std::ostream &out = std::cout) const override;
168 
172  void setup() override;
173 
175  void getAllPlannerDatas(std::vector<ompl::base::PlannerDataPtr> &plannerDatas) const override;
176 
178  std::size_t getExperiencesCount() const override;
179 
185  void convertPlannerData(const ompl::base::PlannerDataPtr &plannerData,
187 
189  const ompl::tools::DynamicTimeWarpPtr &getDynamicTimeWarp() const
190  {
191  return dtw_;
192  }
193 
194  protected:
202 
205 
207  ompl::tools::ParallelPlanPtr pp_;
208 
210  ompl::tools::LightningDBPtr experienceDB_;
211 
213  ompl::tools::DynamicTimeWarpPtr dtw_;
214 
215  }; // end of class Lightning
216 
217  } // end of namespace
218 
219 } // end of namespace
220 #endif
ompl::geometric::LightningRetrieveRepair & getLightningRetrieveRepairPlanner() const
Get a pointer to the retrieve repair planner.
Definition: Lightning.h:125
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
void printLogs(std::ostream &out=std::cout) const override
Display debug data about overall results from Lightning since being loaded.
Definition: Lightning.cpp:403
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:283
A shared pointer wrapper for ompl::base::StateSpace.
void setRepairPlanner(const base::PlannerPtr &planner) override
Set the planner to use for repairing experience paths inside the LightningRetrieveRepair planner...
Definition: Lightning.h:133
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool save() override
Save the experience database to file.
Definition: Lightning.cpp:352
void clear() override
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
Definition: Lightning.cpp:141
ompl::tools::LightningDBPtr experienceDB_
A shared object between all the planners for saving and loading previous experience.
Definition: Lightning.h:210
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: Lightning.cpp:443
ompl::tools::DynamicTimeWarpPtr dtw_
Tool for comparing two paths and scoring them.
Definition: Lightning.h:213
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
bool saveIfChanged() override
Save the experience database to file if there has been a change.
Definition: Lightning.cpp:362
A shared pointer wrapper for ompl::base::Planner.
std::size_t getExperiencesCount() const override
Get the total number of paths stored in the database.
Definition: Lightning.cpp:426
const ompl::tools::DynamicTimeWarpPtr & getDynamicTimeWarp() const
Tool for comparing two paths and scoring them.
Definition: Lightning.h:189
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:280
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
void print(std::ostream &out=std::cout) const override
Print information about the current setup.
Definition: Lightning.cpp:382
A shared pointer wrapper for ompl::base::SpaceInformation.
void printResultsInfo(std::ostream &out=std::cout) const override
Display debug data about potential available solutions.
Definition: Lightning.cpp:372
void setup() override
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: Lightning.cpp:68
The Lightning Framework&#39;s Retrieve-Repair component.
void getAllPlannerDatas(std::vector< ompl::base::PlannerDataPtr > &plannerDatas) const override
Get a vector of all the planning data in the database.
Definition: Lightning.cpp:431
base::PlannerStatus solve(double time=1.0) override
Run the planner for up to a specified amount of time (default is 1 second)
Definition: Lightning.cpp:346
Built off of SimpleSetup but provides support for planning from experience.
Definition: Lightning.h:98
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:289
base::PlannerPtr rrPlanner_
The maintained experience planner instance.
Definition: Lightning.h:204
Definition of a geometric path.
Definition: PathGeometric.h:60
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:433
ompl::tools::ParallelPlanPtr pp_
Instance of parallel planning to use for computing solutions in parallel.
Definition: Lightning.h:207
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: Lightning.h:141
A shared pointer wrapper for ompl::base::PlannerData.
void convertPlannerData(const ompl::base::PlannerDataPtr &plannerData, ompl::geometric::PathGeometric &path)
Convert PlannerData to PathGeometric. Assume ordering of verticies is order of path.
Definition: Lightning.cpp:436
Lightning(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: Lightning.cpp:44
Create the set of classes typically needed to solve a geometric problem.