Loading...
Searching...
No Matches
MaximizeClearanceValidStateSampler.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/samplers/MaximizeClearanceValidStateSampler.h"
38#include "ompl/base/SpaceInformation.h"
39
41 : ValidStateSampler(si), sampler_(si->allocStateSampler()), improveAttempts_(3), work_(si->allocState())
42{
43 name_ = "max_clearance";
44 params_.declareParam<unsigned int>("nr_improve_attempts",
45 [this](unsigned int n)
46 {
48 },
49 [this]
50 {
51 return getNrImproveAttempts();
52 });
53}
54
55ompl::base::MaximizeClearanceValidStateSampler::~MaximizeClearanceValidStateSampler()
56{
57 si_->freeState(work_);
58}
59
61{
62 unsigned int attempts = 0;
63 bool valid = false;
64 double dist = 0.0;
65 do
66 {
67 sampler_->sampleUniform(state);
68 valid = si_->getStateValidityChecker()->isValid(state, dist);
69 ++attempts;
70 } while (!valid && attempts < attempts_);
71
72 if (valid)
73 {
74 bool validW = false;
75 double distW = 0.0;
76 attempts = 0;
77 while (attempts < improveAttempts_)
78 {
79 sampler_->sampleUniform(work_);
80 validW = si_->getStateValidityChecker()->isValid(work_, distW);
81 ++attempts;
82 if (validW && distW > dist)
83 {
84 dist = distW;
85 si_->copyState(state, work_);
86 }
87 }
88 return true;
89 }
90 return false;
91}
92
93bool ompl::base::MaximizeClearanceValidStateSampler::sampleNear(State *state, const State *near, const double distance)
94{
95 unsigned int attempts = 0;
96 bool valid = false;
97 double dist = 0.0;
98 do
99 {
100 sampler_->sampleUniformNear(state, near, distance);
101 valid = si_->getStateValidityChecker()->isValid(state, dist);
102 ++attempts;
103 } while (!valid && attempts < attempts_);
104
105 if (valid)
106 {
107 bool validW = false;
108 double distW = 0.0;
109 attempts = 0;
110 while (attempts < improveAttempts_)
111 {
112 sampler_->sampleUniformNear(work_, near, distance);
113 validW = si_->getStateValidityChecker()->isValid(work_, distW);
114 ++attempts;
115 if (validW && distW > dist)
116 {
117 dist = distW;
118 si_->copyState(state, work_);
119 }
120 }
121 return true;
122 }
123 return false;
124}
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
bool sample(State *state) override
Sample a state. Return false in case of failure.
unsigned int getNrImproveAttempts() const
Get the number of attempts to improve a sampled state.
MaximizeClearanceValidStateSampler(const SpaceInformation *si)
Constructor.
void setNrImproveAttempts(unsigned int attempts)
The number of attempts at improving the clearance of the sampled state.
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=[] { return T();})
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:231
The base class for space information. This contains all the information about the space planning is d...
Definition of an abstract state.
Definition: State.h:50
Abstract definition of a state sampler.
ParamSet params_
The parameters for this instance of the valid state sampler.
std::string name_
The name of the sampler.