Loading...
Searching...
No Matches
AnytimePathShortening.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2014, Rice University
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 Rice University 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: Ryan Luna */
36
37#include <algorithm> // for std::min
38#include <boost/algorithm/string.hpp>
39
40#include "ompl/geometric/planners/AnytimePathShortening.h"
41#include "ompl/geometric/planners/fmt/BFMT.h"
42#include "ompl/geometric/planners/est/BiEST.h"
43#include "ompl/geometric/planners/rrt/BiTRRT.h"
44#include "ompl/geometric/planners/informedtrees/BITstar.h"
45#include "ompl/geometric/planners/kpiece/BKPIECE1.h"
46#include "ompl/geometric/planners/est/EST.h"
47#include "ompl/geometric/planners/fmt/FMT.h"
48#include "ompl/geometric/planners/kpiece/KPIECE1.h"
49#include "ompl/geometric/planners/rrt/LazyLBTRRT.h"
50#include "ompl/geometric/planners/prm/LazyPRM.h"
51#include "ompl/geometric/planners/prm/LazyPRMstar.h"
52#include "ompl/geometric/planners/rrt/LazyRRT.h"
53#include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
54#include "ompl/geometric/planners/rrt/LBTRRT.h"
55#include "ompl/geometric/planners/pdst/PDST.h"
56#include "ompl/geometric/planners/prm/PRM.h"
57#include "ompl/geometric/planners/prm/PRMstar.h"
58#include "ompl/geometric/planners/est/ProjEST.h"
59#include "ompl/geometric/planners/rrt/RRT.h"
60#include "ompl/geometric/planners/rrt/RRTConnect.h"
61#include "ompl/geometric/planners/rrt/RRTstar.h"
62#include "ompl/geometric/planners/rrt/RRTXstatic.h"
63#include "ompl/geometric/planners/sbl/SBL.h"
64#include "ompl/geometric/planners/prm/SPARS.h"
65#include "ompl/geometric/planners/prm/SPARStwo.h"
66#include "ompl/geometric/planners/sst/SST.h"
67#include "ompl/geometric/planners/stride/STRIDE.h"
68#include "ompl/geometric/planners/rrt/TRRT.h"
69#include "ompl/geometric/PathHybridization.h"
70#include "ompl/geometric/PathSimplifier.h"
71#include "ompl/tools/config/SelfConfig.h"
72#include "ompl/base/objectives/PathLengthOptimizationObjective.h"
73#include "ompl/util/String.h"
74
75ompl::geometric::AnytimePathShortening::AnytimePathShortening(const ompl::base::SpaceInformationPtr &si)
76 : ompl::base::Planner(si, "APS"), defaultNumPlanners_(std::max(1u, std::thread::hardware_concurrency()))
77{
78 specs_.approximateSolutions = true;
79 specs_.multithreaded = true;
80 specs_.optimizingPaths = true;
81
82 Planner::declareParam<bool>("shortcut", this, &AnytimePathShortening::setShortcut,
84 Planner::declareParam<bool>("hybridize", this, &AnytimePathShortening::setHybridize,
86 Planner::declareParam<unsigned int>("max_hybrid_paths", this, &AnytimePathShortening::setMaxHybridizationPath,
88 Planner::declareParam<unsigned int>("num_planners", this, &AnytimePathShortening::setDefaultNumPlanners,
90 Planner::declareParam<std::string>("planners", this, &AnytimePathShortening::setPlanners,
92
93 addPlannerProgressProperty("best cost REAL", [this] { return getBestCost(); });
94}
95
97
99{
100 if (planner && planner->getSpaceInformation().get() != si_.get())
101 {
102 OMPL_ERROR("NOT adding planner %s: SpaceInformation instances are different", planner->getName().c_str());
103 return;
104 }
105
106 // Ensure all planners are unique instances
107 for (auto &i : planners_)
108 {
109 if (planner.get() == i.get())
110 {
111 OMPL_ERROR("NOT adding planner %s: Planner instances MUST be unique", planner->getName().c_str());
112 return;
113 }
114 }
115
116 planners_.push_back(planner);
117}
118
119void ompl::geometric::AnytimePathShortening::addPath(const geometric::PathGeometricPtr &path, base::Planner *planner)
120{
121 const base::OptimizationObjectivePtr &opt(pdef_->getOptimizationObjective());
122 base::Cost pathCost = path->cost(opt);
123 std::lock_guard<std::mutex> _(lock_);
124 if (opt->isCostBetterThan(pathCost, bestCost_))
125 {
126 bestCost_ = pathCost;
127 pdef_->addSolutionPath(path, false, 0.0, planner->getName());
128 }
129 else if (planner != this)
130 pdef_->addSolutionPath(path, false, 0.0, planner->getName());
131}
132
135{
136 std::vector<std::thread> threads;
138
139 base::OptimizationObjectivePtr opt = pdef_->getOptimizationObjective();
140 if (!opt)
141 {
142 OMPL_INFORM("%s: No optimization objective specified. Defaulting to optimizing path length for the allowed "
143 "planning time.",
144 getName().c_str());
145 opt = std::make_shared<base::PathLengthOptimizationObjective>(si_);
146 pdef_->setOptimizationObjective(opt);
147 }
148
149 // Disable output from the motion planners, except for errors
150 msg::LogLevel currentLogLevel = msg::getLogLevel();
151 msg::setLogLevel(std::max(msg::LOG_ERROR, currentLogLevel));
152
153 // Clear any previous planning data for the set of planners
154 clear();
155 // Spawn a thread for each planner. This will shortcut the best path after solving.
156 for (auto &planner : planners_)
157 threads.emplace_back([this, planner, &ptc] { return threadSolve(planner.get(), ptc); });
158
160 geometric::PathGeometric *sln = nullptr, *prevLastPath = nullptr;
161 bestCost_ = opt->infiniteCost();
162 std::size_t prevSolCount = 0;
163 while (!ptc)
164 {
165 // We have found a solution that is good enough
166 if (opt->isSatisfied(bestCost_))
167 {
168 ptc.terminate();
169 break;
170 }
171
172 // Hybridize the set of paths computed. Add the new hybrid path to the mix.
173 unsigned int solCount = pdef_->getSolutionCount();
174 if (hybridize_ && !ptc && solCount > 1)
175 {
176 const std::vector<base::PlannerSolution> &paths = pdef_->getSolutions();
177 std::size_t numPaths = std::min(solCount, maxHybridPaths_);
178 geometric::PathGeometric *lastPath = static_cast<PathGeometric *>(paths[numPaths - 1].path_.get());
179 // check if new solution paths have been added to top numPaths paths
180 if (lastPath != prevLastPath || (prevSolCount < solCount && solCount <= maxHybridPaths_))
181 {
182 for (size_t j = 0; j < numPaths && !ptc; ++j)
183 phybrid.recordPath(std::static_pointer_cast<PathGeometric>(paths[j].path_), false);
184
185 phybrid.computeHybridPath();
186 sln = phybrid.getHybridPath().get();
187 prevLastPath = lastPath;
188 }
189 else
190 sln = static_cast<PathGeometric *>(pdef_->getSolutionPath().get());
191 prevSolCount = solCount;
192 }
193 else if (solCount > 0)
194 sln = static_cast<PathGeometric *>(pdef_->getSolutionPath().get());
195
196 if (sln)
197 {
198 auto pathCopy(std::make_shared<geometric::PathGeometric>(*sln));
199 if (shortcut_) // Shortcut the path
200 if (!ps.simplify(*pathCopy, ptc, true))
201 // revert if shortcutting failed
202 pathCopy = std::make_shared<geometric::PathGeometric>(*sln);
203 addPath(pathCopy, this);
204 }
205 if (hybridize_ && phybrid.pathCount() >= maxHybridPaths_)
206 phybrid.clear();
207 }
208
209 for (auto &thread : threads)
210 thread.join();
211
212 msg::setLogLevel(currentLogLevel);
214}
215
218{
219 // create a local clone of the problem definition where we keep at most one solution
220 auto pdef = pdef_->clone();
222
223 planner->setProblemDefinition(pdef);
224 while (!ptc)
225 {
226 if (planner->solve(ptc) == base::PlannerStatus::EXACT_SOLUTION)
227 {
228 geometric::PathGeometric *sln = static_cast<geometric::PathGeometric *>(pdef->getSolutionPath().get());
229 auto pathCopy(std::make_shared<geometric::PathGeometric>(*sln));
230 if (shortcut_) // Shortcut the path
231 ps.shortcutPath(*pathCopy);
232 addPath(pathCopy, planner);
233 }
234
235 planner->clear();
236 pdef->clearSolutionPaths();
237 }
238}
239
241{
242 Planner::clear();
243 for (auto &planner : planners_)
244 planner->clear();
245 bestCost_ = base::Cost(std::numeric_limits<double>::quiet_NaN());
246}
247
249{
250 if (planners_.empty())
251 return;
252
253 OMPL_WARN("Returning planner data for planner #0");
254 getPlannerData(data, 0);
255}
256
258{
259 if (planners_.size() < idx)
260 return;
261 planners_[idx]->getPlannerData(data);
262}
263
265{
266 Planner::setup();
267
268 if (planners_.empty())
269 {
271 for (unsigned int i = 0; i < defaultNumPlanners_; ++i)
272 {
274 planners_.back()->setProblemDefinition(pdef_);
275 }
276 OMPL_INFORM("%s: No planners specified; using %u instances of %s", getName().c_str(), planners_.size(),
277 planners_[0]->getName().c_str());
278 }
279
280 for (auto &planner : planners_)
281 planner->setup();
282}
283
285{
286 for (auto &planner : planners_)
287 planner->checkValidity();
288}
289
291{
292 return planners_.size();
293}
294
295ompl::base::PlannerPtr ompl::geometric::AnytimePathShortening::getPlanner(unsigned int idx) const
296{
297 assert(idx < planners_.size());
298 return planners_[idx];
299}
300
305
307{
308 shortcut_ = shortcut;
309}
310
315
317{
318 hybridize_ = hybridize;
319}
320
325
327{
328 maxHybridPaths_ = maxPathCount;
329}
330
332{
333 defaultNumPlanners_ = numPlanners;
334}
335
340
342{
343 return ompl::toString(bestCost_.value());
344}
345
346void ompl::geometric::AnytimePathShortening::setPlanners(const std::string &plannerList)
347{
348 std::vector<std::string> plannerStrings;
349
350 boost::split(plannerStrings, plannerList, boost::is_any_of(","));
351
352 planners_.clear();
353 for (const auto &plannerString : plannerStrings)
354 {
355 std::vector<std::string> plannerAndParams;
356 boost::split(plannerAndParams, plannerString, boost::is_any_of("[ ]"));
357 const std::string &plannerName = plannerAndParams[0];
358
359 if (plannerName == "BFMT")
360 planners_.push_back(std::make_shared<geometric::BFMT>(si_));
361 else if (plannerName == "BiEST")
362 planners_.push_back(std::make_shared<geometric::BiEST>(si_));
363 else if (plannerName == "BiTRRT")
364 planners_.push_back(std::make_shared<geometric::BiTRRT>(si_));
365 else if (plannerName == "BITstar")
366 planners_.push_back(std::make_shared<geometric::BITstar>(si_));
367 else if (plannerName == "BKPIECE")
368 planners_.push_back(std::make_shared<geometric::BKPIECE1>(si_));
369 else if (plannerName == "EST")
370 planners_.push_back(std::make_shared<geometric::EST>(si_));
371 else if (plannerName == "FMT")
372 planners_.push_back(std::make_shared<geometric::FMT>(si_));
373 else if (plannerName == "KPIECE")
374 planners_.push_back(std::make_shared<geometric::KPIECE1>(si_));
375 else if (plannerName == "LazyLBTRRT")
376 planners_.push_back(std::make_shared<geometric::LazyLBTRRT>(si_));
377 else if (plannerName == "LazyPRM")
378 planners_.push_back(std::make_shared<geometric::LazyPRM>(si_));
379 else if (plannerName == "LazyPRMstar")
380 planners_.push_back(std::make_shared<geometric::LazyPRMstar>(si_));
381 else if (plannerName == "LazyRRT")
382 planners_.push_back(std::make_shared<geometric::LazyRRT>(si_));
383 else if (plannerName == "LBKPIECE")
384 planners_.push_back(std::make_shared<geometric::LBKPIECE1>(si_));
385 else if (plannerName == "LBTRRT")
386 planners_.push_back(std::make_shared<geometric::LBTRRT>(si_));
387 else if (plannerName == "PDST")
388 planners_.push_back(std::make_shared<geometric::PDST>(si_));
389 else if (plannerName == "PRM")
390 planners_.push_back(std::make_shared<geometric::PRM>(si_));
391 else if (plannerName == "PRMstar")
392 planners_.push_back(std::make_shared<geometric::PRMstar>(si_));
393 else if (plannerName == "ProjEST")
394 planners_.push_back(std::make_shared<geometric::ProjEST>(si_));
395 else if (plannerName == "RRT")
396 planners_.push_back(std::make_shared<geometric::RRT>(si_));
397 else if (plannerName == "RRTConnect")
398 planners_.push_back(std::make_shared<geometric::RRTConnect>(si_));
399 else if (plannerName == "RRTstar")
400 planners_.push_back(std::make_shared<geometric::RRTstar>(si_));
401 else if (plannerName == "RRTXstatic")
402 planners_.push_back(std::make_shared<geometric::RRTXstatic>(si_));
403 else if (plannerName == "SBL")
404 planners_.push_back(std::make_shared<geometric::SBL>(si_));
405 else if (plannerName == "SPARS")
406 planners_.push_back(std::make_shared<geometric::SPARS>(si_));
407 else if (plannerName == "SPARStwo")
408 planners_.push_back(std::make_shared<geometric::SPARStwo>(si_));
409 else if (plannerName == "SST")
410 planners_.push_back(std::make_shared<geometric::SST>(si_));
411 else if (plannerName == "STRIDE")
412 planners_.push_back(std::make_shared<geometric::STRIDE>(si_));
413 else if (plannerName == "TRRT")
414 planners_.push_back(std::make_shared<geometric::TRRT>(si_));
415 else
416 OMPL_ERROR("Unknown planner name: %s", plannerName.c_str());
417
418 std::vector<std::string> paramValue;
419 for (auto it = plannerAndParams.begin() + 1; it != plannerAndParams.end(); ++it)
420 if (!it->empty())
421 {
422 boost::split(paramValue, *it, boost::is_any_of("="));
423 planners_.back()->params().setParam(paramValue[0], paramValue[1]);
424 }
425 }
426}
427
429{
430 std::stringstream ss;
431 for (unsigned int i = 0; i < planners_.size(); ++i)
432 {
433 if (i > 0)
434 ss << ',';
435 ss << planners_[i]->getName();
436
437 std::map<std::string, std::string> params;
438 planners_[i]->params().getParams(params);
439 if (params.size() > 0)
440 {
441 ss << '[';
442 for (auto it = params.begin(); it != params.end(); ++it)
443 {
444 if (it != params.begin())
445 ss << ' ';
446 ss << it->first << '=' << it->second;
447 }
448 ss << ']';
449 }
450 }
451 return ss.str();
452}
453
455{
456 Planner::printSettings(out);
457 out << "Settings for planner instances in AnytimePathShortening instance:\n";
458 for (const auto &planner : planners_)
459 {
460 out << "* ";
461 planner->printSettings(out);
462 }
463}
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition Cost.h:48
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void terminate() const
Notify that the condition for termination should become true, regardless of what eval() returns....
Base class for a planner.
Definition Planner.h:216
ParamSet & params()
Get the parameters for this planner.
Definition Planner.h:336
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition Planner.h:403
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
const std::string & getName() const
Get the name of the planner.
Definition Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
bool hybridize_
Flag indicating whether to hybridize the set of solution paths.
base::PlannerPtr getPlanner(unsigned int idx) const
Retrieve a pointer to the ith planner instance.
~AnytimePathShortening() override
Destructor.
unsigned int defaultNumPlanners_
The number of planners to use if none are specified. This defaults to the number of cores....
void clear() override
Clear all internal planning datastructures. Planner settings are not affected. Subsequent calls to so...
std::string getBestCost() const
Return best cost found so far by algorithm.
void setPlanners(const std::string &plannerList)
Set the list of planners to use.
void getPlannerData(base::PlannerData &data) const override
Get information about the most recent run of the motion planner.
unsigned int maxHybridizationPaths() const
Return the maximum number of paths that will be hybridized.
virtual void threadSolve(base::Planner *planner, const base::PlannerTerminationCondition &ptc)
The function that the planning threads execute when solving a motion planning problem.
void setDefaultNumPlanners(unsigned int numPlanners)
Set default number of planners to use if none are specified.
bool shortcut_
Flag indicating whether to shortcut paths.
unsigned int maxHybridPaths_
The maximum number of paths that will be hybridized. This prohibits hybridization of a very large pat...
AnytimePathShortening(const base::SpaceInformationPtr &si)
Constructor requires the space information to plan in.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Method that solves the motion planning problem. This method terminates under just two conditions,...
void printSettings(std::ostream &out) const override
Print settings of this planner as well as those of the planner instances it contains.
std::mutex lock_
mutex for updating bestCost_
void setup() override
Perform any necessary configuration steps. This method also invokes ompl::base::SpaceInformation::set...
bool isShortcutting() const
Return whether the anytime planner will perform shortcutting on paths.
bool isHybridizing() const
Return whether the anytime planner will extract a hybrid path from the set of solution paths.
void setMaxHybridizationPath(unsigned int maxPathCount)
Set the maximum number of paths that will be hybridized.
std::string getPlanners() const
Get a string representation of the planners and their parameters in the format of setPlanners.
void addPath(const geometric::PathGeometricPtr &path, base::Planner *planner)
add a path to set of solutions
unsigned int getNumPlanners() const
Retrieve the number of planners added.
void addPlanner(base::PlannerPtr &planner)
Adds the given planner to the set of planners used to compute candidate paths.
base::Cost bestCost_
Best cost found so far by algorithm.
unsigned int getDefaultNumPlanners() const
Get default number of planners used if none are specified.
void setHybridize(bool hybridize)
Enable/disable path hybridization on the set of solution paths.
std::vector< base::PlannerPtr > planners_
The list of planners used for solving the problem.
void setShortcut(bool shortcut)
Enable/disable shortcutting on paths.
void checkValidity() override
Check to see if the planners are in a working state (setup has been called, a goal was set,...
Definition of a geometric path.
Given multiple geometric paths, attempt to combine them in order to obtain a shorter solution.
const geometric::PathGeometricPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
void clear()
Clear all the stored paths.
void computeHybridPath()
Run Dijkstra's algorithm to find out the lowest-cost path among the mixed ones.
unsigned int recordPath(const geometric::PathGeometricPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
This class contains routines that attempt to simplify geometric paths.
bool simplify(PathGeometric &path, double maxTime, bool atLeastOnce=true)
Run simplification algorithms on the path for at most maxTime seconds, and at least once if atLeastOn...
bool shortcutPath(PathGeometric &path, unsigned int maxSteps=0, unsigned int maxEmptySteps=0, double rangeRatio=0.33, double snapToVertex=0.005)
Given a path, attempt to shorten it while maintaining its validity. This is an iterative process that...
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
#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...
void setLogLevel(LogLevel level)
Set the minimum level of logging data to output. Messages with lower logging levels will not be recor...
Definition Console.cpp:136
LogLevel
The set of priorities for message logging.
Definition Console.h:85
LogLevel getLogLevel()
Retrieve the current level of logging data. Messages with lower logging levels will not be recorded.
Definition Console.cpp:142
Main namespace. Contains everything in this library.
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition String.cpp:82
STL namespace.
A class to store the exit status of Planner::solve()
@ EXACT_SOLUTION
The planner found an exact solution.
@ UNKNOWN
Uninitialized status.