Loading...
Searching...
No Matches
SelfConfig.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2011, 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: Ioan Sucan */
36
37#include "ompl/tools/config/SelfConfig.h"
38#include "ompl/tools/config/MagicConstants.h"
39#include "ompl/geometric/planners/rrt/RRTConnect.h"
40#include "ompl/geometric/planners/rrt/RRT.h"
41#include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
42#include "ompl/geometric/planners/kpiece/KPIECE1.h"
43#include "ompl/control/planners/rrt/RRT.h"
44#include "ompl/control/planners/kpiece/KPIECE1.h"
45#include "ompl/util/Console.h"
46#include <memory>
47#include <algorithm>
48#include <limits>
49#include <cmath>
50#include <map>
51
53namespace ompl
54{
55 namespace tools
56 {
57 class SelfConfig::SelfConfigImpl
58 {
59 friend class SelfConfig;
60
61 public:
62 SelfConfigImpl(const base::SpaceInformationPtr &si)
63 : wsi_(si), probabilityOfValidState_(-1.0), averageValidMotionLength_(-1.0)
64 {
65 }
66
67 double getProbabilityOfValidState()
68 {
69 base::SpaceInformationPtr si = wsi_.lock();
70 checkSetup(si);
71 if (si && probabilityOfValidState_ < 0.0)
72 probabilityOfValidState_ = si->probabilityOfValidState(magic::TEST_STATE_COUNT);
73 return probabilityOfValidState_;
74 }
75
76 double getAverageValidMotionLength()
77 {
78 base::SpaceInformationPtr si = wsi_.lock();
79 checkSetup(si);
80 if (si && averageValidMotionLength_ < 0.0)
81 averageValidMotionLength_ = si->averageValidMotionLength(magic::TEST_STATE_COUNT);
82 return averageValidMotionLength_;
83 }
84
85 void configureValidStateSamplingAttempts(unsigned int &attempts)
86 {
87 if (attempts == 0)
88 attempts = magic::MAX_VALID_SAMPLE_ATTEMPTS;
89 }
90
91 void configurePlannerRange(double &range, const std::string &context)
92 {
93 if (range < std::numeric_limits<double>::epsilon())
94 {
95 base::SpaceInformationPtr si = wsi_.lock();
96 if (si)
97 {
98 range = si->getMaximumExtent() * magic::MAX_MOTION_LENGTH_AS_SPACE_EXTENT_FRACTION;
99 OMPL_DEBUG("%sPlanner range detected to be %lf", context.c_str(), range);
100 }
101 else
102 OMPL_ERROR("%sUnable to detect planner range. SpaceInformation instance has expired.",
103 context.c_str());
104 }
105 }
106
107 void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj, const std::string &context)
108 {
109 base::SpaceInformationPtr si = wsi_.lock();
110 checkSetup(si);
111 if (!proj && si)
112 {
113 OMPL_INFORM("%sAttempting to use default projection.", context.c_str());
114 proj = si->getStateSpace()->getDefaultProjection();
115 }
116 if (!proj)
117 throw Exception("No projection evaluator specified");
118 proj->setup();
119 }
120
121 void print(std::ostream &out) const
122 {
123 base::SpaceInformationPtr si = wsi_.lock();
124 if (si)
125 {
126 out << "Configuration parameters for space '" << si->getStateSpace()->getName() << "'" << std::endl;
127 out << " - probability of a valid state is " << probabilityOfValidState_ << std::endl;
128 out << " - average length of a valid motion is " << averageValidMotionLength_ << std::endl;
129 }
130 else
131 out << "EXPIRED" << std::endl;
132 }
133
134 bool expired() const
135 {
136 return wsi_.expired();
137 }
138
139 private:
140 void checkSetup(const base::SpaceInformationPtr &si)
141 {
142 if (si)
143 {
144 if (!si->isSetup())
145 {
146 si->setup();
147 probabilityOfValidState_ = -1.0;
148 averageValidMotionLength_ = -1.0;
149 }
150 }
151 else
152 {
153 probabilityOfValidState_ = -1.0;
154 averageValidMotionLength_ = -1.0;
155 }
156 }
157
158 // we store weak pointers so that the SpaceInformation instances are not kept in
159 // memory until termination of the program due to the use of a static ConfigMap below
160 std::weak_ptr<base::SpaceInformation> wsi_;
161
162 double probabilityOfValidState_;
163 double averageValidMotionLength_;
164
165 std::mutex lock_;
166 };
167 }
168}
169
170std::mutex ompl::tools::SelfConfig::staticConstructorLock_;
172
173ompl::tools::SelfConfig::SelfConfig(const base::SpaceInformationPtr &si, const std::string &context)
174 : context_(context.empty() ? "" : context + ": ")
175{
176 using ConfigMap = std::map<base::SpaceInformation *, std::shared_ptr<SelfConfigImpl>>;
177
178 std::unique_lock<std::mutex> smLock(staticConstructorLock_);
179
180 static ConfigMap SMAP;
181
182 // clean expired entries from the map
183 auto dit = SMAP.begin();
184 while (dit != SMAP.end())
185 {
186 if (dit->second->expired())
187 SMAP.erase(dit++);
188 else
189 ++dit;
190 }
191
192 const auto it = SMAP.find(si.get());
193
194 if (it != SMAP.end())
195 impl_ = it->second.get();
196 else
197 {
198 impl_ = new SelfConfigImpl(si);
199 SMAP[si.get()].reset(impl_);
200 }
201}
202
203ompl::tools::SelfConfig::~SelfConfig() = default;
204
205/* ------------------------------------------------------------------------ */
206
208{
209 std::lock_guard<std::mutex> iLock(impl_->lock_);
210 return impl_->getProbabilityOfValidState();
211}
212
214{
215 std::lock_guard<std::mutex> iLock(impl_->lock_);
216 return impl_->getAverageValidMotionLength();
217}
218
220{
221 std::lock_guard<std::mutex> iLock(impl_->lock_);
222 impl_->configureValidStateSamplingAttempts(attempts);
223}
224
226{
227 std::lock_guard<std::mutex> iLock(impl_->lock_);
228 impl_->configurePlannerRange(range, context_);
229}
230
231void ompl::tools::SelfConfig::configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
232{
233 std::lock_guard<std::mutex> iLock(impl_->lock_);
234 return impl_->configureProjectionEvaluator(proj, context_);
235}
236
237void ompl::tools::SelfConfig::print(std::ostream &out) const
238{
239 std::lock_guard<std::mutex> iLock(impl_->lock_);
240 impl_->print(out);
241}
242
244{
245 base::PlannerPtr planner;
246 base::SpaceInformationPtr si(goal->getSpaceInformation());
247 const base::StateSpacePtr &space(si->getStateSpace());
248 control::SpaceInformationPtr siC(std::dynamic_pointer_cast<control::SpaceInformation, base::SpaceInformation>(si));
249 if (siC) // kinodynamic planning
250 {
251 // if we have a default projection
252 if (space->hasDefaultProjection())
253 planner = std::make_shared<control::KPIECE1>(siC);
254 // otherwise use a single-tree planner
255 else
256 planner = std::make_shared<control::RRT>(siC);
257 }
258 // if we can sample the goal region and interpolation between states is symmetric,
259 // use a bi-directional planner
260 else if (!goal)
261 {
262 OMPL_WARN("No goal specified; choosing RRT as the default planner");
263 planner = std::make_shared<geometric::RRT>(goal->getSpaceInformation());
264 }
265 else if (goal->hasType(base::GOAL_SAMPLEABLE_REGION) && space->hasSymmetricInterpolate())
266 {
267 // if we have a default projection
268 if (space->hasDefaultProjection())
269 planner = std::make_shared<geometric::LBKPIECE1>(goal->getSpaceInformation());
270 else
271 planner = std::make_shared<geometric::RRTConnect>(goal->getSpaceInformation());
272 }
273 // otherwise use a single-tree planner
274 else
275 {
276 // if we have a default projection
277 if (space->hasDefaultProjection())
278 planner = std::make_shared<geometric::KPIECE1>(goal->getSpaceInformation());
279 else
280 planner = std::make_shared<geometric::RRT>(goal->getSpaceInformation());
281 }
282
283 if (!planner)
284 throw Exception("Unable to allocate default planner");
285
286 return planner;
287}
The exception type for ompl.
Definition: Exception.h:47
A shared pointer wrapper for ompl::base::Planner.
void configureValidStateSamplingAttempts(unsigned int &attempts)
Instances of base::ValidStateSampler need a number of attempts to be specified – the maximum number o...
Definition: SelfConfig.cpp:219
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:225
double getProbabilityOfValidState()
Get the probability of a sampled state being valid (calls base::SpaceInformation::probabilityOfValidS...
Definition: SelfConfig.cpp:207
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:231
double getAverageValidMotionLength()
Get the probability of a sampled state being valid (calls base::SpaceInformation::averageValidMotionL...
Definition: SelfConfig.cpp:213
void print(std::ostream &out=std::cout) const
Print the computed configuration parameters.
Definition: SelfConfig.cpp:237
SelfConfig(const base::SpaceInformationPtr &si, const std::string &context=std::string())
Construct an instance that can configure the space encapsulated by si. Any information printed to the...
Definition: SelfConfig.cpp:173
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:243
#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_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
Main namespace. Contains everything in this library.