Loading...
Searching...
No Matches
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
68namespace ompl
69{
70 namespace tools
71 {
72 // class LightningDB; // forward declaration
73 OMPL_CLASS_FORWARD(LightningDB);
74 OMPL_CLASS_FORWARD(ParallelPlan);
75
91 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
204 base::PlannerPtr rrPlanner_;
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
A shared pointer wrapper for ompl::base::PlannerData.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
The Lightning Framework's Retrieve-Repair component.
void setRepairPlanner(const base::PlannerPtr &planner)
Set the planner that will be used for repairing invalid paths recalled from experience.
Definition of a geometric path.
bool configured_
Flag indicating whether the classes needed for planning are set up.
base::PlannerPtr planner_
The maintained planner instance.
base::PlannerAllocator pa_
The optional planner allocator.
Create the set of classes typically needed to solve a geometric problem.
Built off of SimpleSetup but provides support for planning from experience.
Definition Lightning.h:99
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 verticies is order of path.
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
const ompl::tools::DynamicTimeWarpPtr & getDynamicTimeWarp() const
Tool for comparing two paths and scoring them.
Definition Lightning.h:189
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.
void setRepairPlanner(const base::PlannerPtr &planner) override
Set the planner to use for repairing experience paths inside the LightningRetrieveRepair planner....
Definition Lightning.h:133
ompl::tools::DynamicTimeWarpPtr dtw_
Tool for comparing two paths and scoring them.
Definition Lightning.h:213
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition Planner.h:444
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()