constrained_sampler.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage 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 
40 
42  const constraint_samplers::ConstraintSamplerPtr& cs)
43  : ob::StateSampler(pc->getOMPLStateSpace().get())
44  , planning_context_(pc)
45  , default_(space_->allocDefaultStateSampler())
46  , constraint_sampler_(cs)
47  , work_state_(pc->getCompleteInitialRobotState())
48  , constrained_success_(0)
49  , constrained_failure_(0)
50 {
51  inv_dim_ = space_->getDimension() > 0 ? 1.0 / (double)space_->getDimension() : 1.0;
52 }
53 
55 {
56  if (constrained_success_ == 0)
57  return 0.0;
58  else
60 }
61 
63 {
64  // moveit::Profiler::ScopedBlock sblock("sampleWithConstraints");
65 
68  {
69  planning_context_->getOMPLStateSpace()->copyToOMPLState(state, work_state_);
70  if (space_->satisfiesBounds(state))
71  {
73  return true;
74  }
75  }
77  return false;
78 }
79 
81 {
82  if (!sampleC(state) && !sampleC(state) && !sampleC(state))
83  default_->sampleUniform(state);
84 }
85 
86 void ompl_interface::ConstrainedSampler::sampleUniformNear(ob::State* state, const ob::State* near,
87  const double distance)
88 {
89  if (sampleC(state) || sampleC(state) || sampleC(state))
90  {
91  double total_d = space_->distance(state, near);
92  if (total_d > distance)
93  {
94  double dist = pow(rng_.uniform01(), inv_dim_) * distance;
95  space_->interpolate(near, state, dist / total_d, state);
96  }
97  }
98  else
99  default_->sampleUniformNear(state, near, distance);
100 }
101 
102 void ompl_interface::ConstrainedSampler::sampleGaussian(ob::State* state, const ob::State* mean, const double stdDev)
103 {
104  if (sampleC(state) || sampleC(state) || sampleC(state))
105  {
106  double total_d = space_->distance(state, mean);
107  double distance = rng_.gaussian(0.0, stdDev);
108  if (total_d > distance)
109  {
110  double dist = pow(rng_.uniform01(), inv_dim_) * distance;
111  space_->interpolate(mean, state, dist / total_d, state);
112  }
113  }
114  else
115  default_->sampleGaussian(state, mean, stdDev);
116 }
constraint_samplers::ConstraintSamplerPtr constraint_sampler_
ompl::base::StateSamplerPtr default_
ConstrainedSampler(const ModelBasedPlanningContext *pc, const constraint_samplers::ConstraintSamplerPtr &cs)
Default constructor.
const robot_state::RobotState & getCompleteInitialRobotState() const
virtual void sampleGaussian(ompl::base::State *state, const ompl::base::State *mean, const double stdDev)
Sample a state using the specified Gaussian.
virtual void sampleUniformNear(ompl::base::State *state, const ompl::base::State *near, const double distance)
Sample a state (uniformly) within a certain distance of another state.
def get(url)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
bool sampleC(ompl::base::State *state)
virtual void sampleUniform(ompl::base::State *state)
Sample a state (uniformly)
double distance(const urdf::Pose &transform)
const ModelBasedPlanningContext * planning_context_
const ModelBasedStateSpacePtr & getOMPLStateSpace() const


ompl
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:01