Loading...
Searching...
No Matches
SpaceInformation.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2010, 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/base/SpaceInformation.h"
38#include <cassert>
39#include <queue>
40#include <utility>
41#include "ompl/base/DiscreteMotionValidator.h"
42#include "ompl/base/samplers/UniformValidStateSampler.h"
43#include "ompl/base/spaces/DubinsStateSpace.h"
44#include "ompl/base/spaces/ReedsSheppStateSpace.h"
45#include "ompl/base/spaces/constraint/ConstrainedStateSpace.h"
46#include "ompl/tools/config/MagicConstants.h"
47#include "ompl/util/Exception.h"
48#include "ompl/util/Time.h"
49
50ompl::base::SpaceInformation::SpaceInformation(StateSpacePtr space) : stateSpace_(std::move(space)), setup_(false)
51{
52 if (!stateSpace_)
53 throw Exception("Invalid space definition");
55 params_.include(stateSpace_->params());
56}
57
59{
60 if (!stateValidityChecker_)
61 {
62 stateValidityChecker_ = std::make_shared<AllValidStateValidityChecker>(this);
63 OMPL_WARN("State validity checker not set! No collision checking is performed");
64 }
65
66 if (!motionValidator_)
67 setDefaultMotionValidator();
68
69 stateSpace_->setup();
70 if (stateSpace_->getDimension() <= 0)
71 throw Exception("The dimension of the state space we plan in must be > 0");
72
73 params_.clear();
74 params_.include(stateSpace_->params());
75
76 setup_ = true;
77}
78
80{
81 return setup_;
82}
83
85{
86 class FnStateValidityChecker : public StateValidityChecker
87 {
88 public:
89 FnStateValidityChecker(SpaceInformation *si, StateValidityCheckerFn fn)
90 : StateValidityChecker(si), fn_(std::move(fn))
91 {
92 }
93
94 bool isValid(const State *state) const override
95 {
96 return fn_(state);
97 }
98
99 protected:
101 };
102
103 if (!svc)
104 throw Exception("Invalid function definition for state validity checking");
105
106 setStateValidityChecker(std::make_shared<FnStateValidityChecker>(this, svc));
107}
108
110{
111 if (dynamic_cast<ReedsSheppStateSpace *>(stateSpace_.get()))
112 motionValidator_ = std::make_shared<ReedsSheppMotionValidator>(this);
113 else if (dynamic_cast<DubinsStateSpace *>(stateSpace_.get()))
114 motionValidator_ = std::make_shared<DubinsMotionValidator>(this);
115 else if (dynamic_cast<ConstrainedStateSpace *>(stateSpace_.get()))
116 motionValidator_ = std::make_shared<ConstrainedMotionValidator>(this);
117 else
118 motionValidator_ = std::make_shared<DiscreteMotionValidator>(this);
119}
120
122{
123 vssa_ = vssa;
124 setup_ = false;
125}
126
132
134 unsigned int steps, std::vector<State *> &states,
135 bool alloc) const
136{
137 if (alloc)
138 {
139 states.resize(steps);
140 for (unsigned int i = 0; i < steps; ++i)
141 states[i] = allocState();
142 }
143 else if (states.size() < steps)
144 steps = states.size();
145
146 const State *prev = start;
147 std::pair<State *, double> lastValid;
148 unsigned int j = 0;
149 for (unsigned int i = 0; i < steps; ++i)
150 {
151 sss->sampleUniform(states[j]);
152 lastValid.first = states[j];
153 if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
154 prev = states[j++];
155 }
156
157 return j;
158}
159
161 const State *near, double distance) const
162{
163 if (state != near)
164 copyState(state, near);
165
166 // fix bounds, if needed
167 if (!satisfiesBounds(state))
168 enforceBounds(state);
169
170 bool result = isValid(state);
171
172 if (!result)
173 {
174 // try to find a valid state nearby
175 State *temp = cloneState(state);
176 result = sampler->sampleNear(state, temp, distance);
177 freeState(temp);
178 }
179
180 return result;
181}
182
183bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance,
184 unsigned int attempts) const
185{
186 if (satisfiesBounds(near) && isValid(near))
187 {
188 if (state != near)
189 copyState(state, near);
190 return true;
191 }
192 else
193 {
194 // try to find a valid state nearby
195 auto uvss = std::make_shared<UniformValidStateSampler>(this);
196 uvss->setNrAttempts(attempts);
197 return searchValidNearby(uvss, state, near, distance);
198 }
199}
200
202 std::vector<State *> &states, unsigned int count,
203 bool endpoints, bool alloc) const
204{
205 // add 1 to the number of states we want to add between s1 & s2. This gives us the number of segments to split the
206 // motion into
207 count++;
208
209 if (count < 2)
210 {
211 unsigned int added = 0;
212
213 // if they want endpoints, then at most endpoints are included
214 if (endpoints)
215 {
216 if (alloc)
217 {
218 states.resize(2);
219 states[0] = allocState();
220 states[1] = allocState();
221 }
222 if (states.size() > 0)
223 {
224 copyState(states[0], s1);
225 added++;
226 }
227
228 if (states.size() > 1)
229 {
230 copyState(states[1], s2);
231 added++;
232 }
233 }
234 else if (alloc)
235 states.resize(0);
236 return added;
237 }
238
239 if (alloc)
240 {
241 states.resize(count + (endpoints ? 1 : -1));
242 if (endpoints)
243 states[0] = allocState();
244 }
245
246 unsigned int added = 0;
247
248 if (endpoints && states.size() > 0)
249 {
250 copyState(states[0], s1);
251 added++;
252 }
253
254 /* find the states in between */
255 for (unsigned int j = 1; j < count && added < states.size(); ++j)
256 {
257 if (alloc)
258 states[added] = allocState();
259 stateSpace_->interpolate(s1, s2, (double)j / (double)count, states[added]);
260 added++;
261 }
262
263 if (added < states.size() && endpoints)
264 {
265 if (alloc)
266 states[added] = allocState();
267 copyState(states[added], s2);
268 added++;
269 }
270
271 return added;
272}
273
274bool ompl::base::SpaceInformation::checkMotion(const std::vector<State *> &states, unsigned int count,
275 unsigned int &firstInvalidStateIndex) const
276{
277 assert(states.size() >= count);
278 for (unsigned int i = 0; i < count; ++i)
279 if (!isValid(states[i]))
280 {
281 firstInvalidStateIndex = i;
282 return false;
283 }
284 return true;
285}
286
287bool ompl::base::SpaceInformation::checkMotion(const std::vector<State *> &states, unsigned int count) const
288{
289 assert(states.size() >= count);
290 if (count > 0)
291 {
292 if (count > 1)
293 {
294 if (!isValid(states.front()))
295 return false;
296 if (!isValid(states[count - 1]))
297 return false;
298
299 // we have 2 or more states, and the first and last states are valid
300
301 if (count > 2)
302 {
303 std::queue<std::pair<int, int>> pos;
304 pos.emplace(0, count - 1);
305
306 while (!pos.empty())
307 {
308 std::pair<int, int> x = pos.front();
309
310 int mid = (x.first + x.second) / 2;
311 if (!isValid(states[mid]))
312 return false;
313
314 pos.pop();
315
316 if (x.first < mid - 1)
317 pos.emplace(x.first, mid);
318 if (x.second > mid + 1)
319 pos.emplace(mid, x.second);
320 }
321 }
322 }
323 else
324 return isValid(states.front());
325 }
326 return true;
327}
328
330{
331 if (vssa_)
332 return vssa_(this);
333 else
334 return std::make_shared<UniformValidStateSampler>(this);
335}
336
338{
339 if (attempts == 0)
340 return 0.0;
341
342 unsigned int valid = 0;
343 unsigned int invalid = 0;
344
345 StateSamplerPtr ss = allocStateSampler();
346 State *s = allocState();
347
348 for (unsigned int i = 0; i < attempts; ++i)
349 {
350 ss->sampleUniform(s);
351 if (isValid(s))
352 ++valid;
353 else
354 ++invalid;
355 }
356
357 freeState(s);
358
359 return (double)valid / (double)(valid + invalid);
360}
361
363{
364 // take the square root here because we in fact have a nested for loop
365 // where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop
366 // too)
367 attempts = std::max((unsigned int)floor(sqrt((double)attempts) + 0.5), 2u);
368
369 StateSamplerPtr ss = allocStateSampler();
370 auto uvss(std::make_shared<UniformValidStateSampler>(this));
371 uvss->setNrAttempts(attempts);
372
373 State *s1 = allocState();
374 State *s2 = allocState();
375
376 std::pair<State *, double> lastValid;
377 lastValid.first = nullptr;
378
379 double d = 0.0;
380 unsigned int count = 0;
381 for (unsigned int i = 0; i < attempts; ++i)
382 if (uvss->sample(s1))
383 {
384 ++count;
385 ss->sampleUniform(s2);
386 if (checkMotion(s1, s2, lastValid))
387 d += distance(s1, s2);
388 else
389 d += distance(s1, s2) * lastValid.second;
390 }
391
392 freeState(s2);
393 freeState(s1);
394
395 if (count > 0)
396 return d / (double)count;
397 else
398 return 0.0;
399}
400
401void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian,
402 unsigned int attempts) const
403{
404 StateSamplerPtr ss = allocStateSampler();
405 std::vector<State *> states(attempts + 1);
406 allocStates(states);
407
408 time::point start = time::now();
409 for (unsigned int i = 0; i < attempts; ++i)
410 ss->sampleUniform(states[i]);
411 uniform = (double)attempts / time::seconds(time::now() - start);
412
413 double d = getMaximumExtent() / 10.0;
414 ss->sampleUniform(states[attempts]);
415
416 start = time::now();
417 for (unsigned int i = 1; i <= attempts; ++i)
418 ss->sampleUniformNear(states[i - 1], states[i], d);
419 near = (double)attempts / time::seconds(time::now() - start);
420
421 start = time::now();
422 for (unsigned int i = 1; i <= attempts; ++i)
423 ss->sampleGaussian(states[i - 1], states[i], d);
424 gaussian = (double)attempts / time::seconds(time::now() - start);
425
426 freeStates(states);
427}
428
430{
431 out << "Settings for the state space '" << stateSpace_->getName() << "'" << std::endl;
432 out << " - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%'
433 << std::endl;
434 out << " - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl;
435 out << " - state space:" << std::endl;
436 stateSpace_->printSettings(out);
437 out << std::endl << "Declared parameters:" << std::endl;
438 params_.print(out);
439 ValidStateSamplerPtr vss = allocValidStateSampler();
440 out << "Valid state sampler named " << vss->getName() << " with parameters:" << std::endl;
441 vss->params().print(out);
442}
443
445{
446 out << "Properties of the state space '" << stateSpace_->getName() << "'" << std::endl;
447 out << " - signature: ";
448 std::vector<int> sig;
449 stateSpace_->computeSignature(sig);
450 for (int i : sig)
451 out << i << " ";
452 out << std::endl;
453 out << " - dimension: " << stateSpace_->getDimension() << std::endl;
454 out << " - extent: " << stateSpace_->getMaximumExtent() << std::endl;
455 if (isSetup())
456 {
457 bool result = true;
458 try
459 {
460 stateSpace_->sanityChecks();
461 }
462 catch (Exception &e)
463 {
464 result = false;
465 out << std::endl
466 << " - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl
467 << std::endl;
468 OMPL_ERROR(e.what());
469 }
470 if (result)
471 out << " - sanity checks for state space passed" << std::endl;
472 out << " - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
473 out << " - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT)
474 << std::endl;
475 double uniform, near, gaussian;
476 samplesPerSecond(uniform, near, gaussian, magic::TEST_STATE_COUNT);
477 out << " - average number of samples drawn per second: sampleUniform()=" << uniform
478 << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl;
479 }
480 else
481 out << "Call setup() before to get more information" << std::endl;
482}
The exception type for ompl.
Definition Exception.h:47
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
An SE(2) state space where distance is measured by the length of Dubins curves.
void include(const ParamSet &other, const std::string &prefix="")
Include the params of a different ParamSet into this one. Optionally include a prefix for each of the...
An SE(2) state space where distance is measured by the length of Reeds-Shepp curves.
The base class for space information. This contains all the information about the space planning is d...
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
void setDefaultMotionValidator()
Set default motion validator for the state space.
virtual unsigned int getMotionStates(const State *s1, const State *s2, std::vector< State * > &states, unsigned int count, bool endpoints, bool alloc) const
Get count states that make up a motion between s1 and s2. Returns the number of states that were adde...
void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const
Estimate the number of samples that can be drawn per second, using the sampler returned by allocState...
bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
Find a valid state near a given one. If the given state is valid, it will be returned itself....
double probabilityOfValidState(unsigned int attempts) const
Estimate probability of sampling a valid state. setup() is assumed to have been called.
virtual void setup()
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
double averageValidMotionLength(unsigned int attempts) const
Estimate the length of a valid motion. setup() is assumed to have been called.
ParamSet params_
Combined parameters for the contained classes.
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
bool isSetup() const
Return true if setup was called.
virtual void printProperties(std::ostream &out=std::cout) const
Print properties of the current instance of the state space.
StateSpacePtr stateSpace_
The state space planning is to be performed in.
unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector< State * > &states, bool alloc) const
Produce a valid motion starting at start by randomly bouncing off of invalid states....
ValidStateSamplerPtr allocValidStateSampler() const
Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() was ...
void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
Set the allocator to use for a valid state sampler. This replaces the default uniform valid state sam...
void clearValidStateSamplerAllocator()
Clear the allocator used for the valid state sampler. This will revert to using the uniform valid sta...
virtual bool checkMotion(const State *s1, const State *s2, std::pair< State *, double > &lastValid) const
Incrementally check if the path between two motions is valid. Also compute the last state that was va...
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Definition of an abstract state.
Definition State.h:50
A shared pointer wrapper for ompl::base::ValidStateSampler.
#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
std::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
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
STL namespace.