constrained_valid_state_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 ModelBasedPlanningContext* pc, const kinematic_constraints::KinematicConstraintSetPtr& ks,
43  const constraint_samplers::ConstraintSamplerPtr& cs)
44  : ob::ValidStateSampler(pc->getOMPLSimpleSetup()->getSpaceInformation().get())
45  , planning_context_(pc)
46  , kinematic_constraint_set_(ks)
47  , constraint_sampler_(cs)
48  , work_state_(pc->getCompleteInitialRobotState())
49 {
51  default_sampler_ = si_->allocStateSampler();
52  inv_dim_ = si_->getStateSpace()->getDimension() > 0 ? 1.0 / (double)si_->getStateSpace()->getDimension() : 1.0;
53  ROS_DEBUG_NAMED("constrained_valid_state_sampler", "Constructed a ValidConstrainedSampler instance at address %p",
54  this);
55 }
56 
58 {
60  {
61  planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, state);
63  {
64  if (kinematic_constraint_set_->decide(work_state_).satisfied)
65  {
66  planning_context_->getOMPLStateSpace()->copyToOMPLState(state, work_state_);
67  return true;
68  }
69  }
70  }
71  return false;
72 }
73 
75 {
76  // moveit::Profiler::ScopedBlock pblock("ValidConstrainedSampler::sample");
78  {
81  {
82  if (kinematic_constraint_set_->decide(work_state_).satisfied)
83  {
84  planning_context_->getOMPLStateSpace()->copyToOMPLState(state, work_state_);
85  return true;
86  }
87  }
88  }
89  else
90  {
91  default_sampler_->sampleUniform(state);
92  planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, state);
93  if (kinematic_constraint_set_->decide(work_state_).satisfied)
94  return true;
95  }
96 
97  return false;
98 }
99 
100 bool ompl_interface::ValidConstrainedSampler::sampleNear(ompl::base::State* state, const ompl::base::State* near,
101  const double distance)
102 {
103  if (!sample(state))
104  return false;
105  double total_d = si_->distance(state, near);
106  if (total_d > distance)
107  {
108  double dist = pow(rng_.uniform01(), inv_dim_) * distance;
109  si_->getStateSpace()->interpolate(near, state, dist / total_d, state);
110  planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, state);
111  if (!kinematic_constraint_set_->decide(work_state_).satisfied)
112  return false;
113  }
114  return true;
115 }
const ModelBasedPlanningContext * planning_context_
virtual bool sampleNear(ompl::base::State *state, const ompl::base::State *near, const double distance)
kinematic_constraints::KinematicConstraintSetPtr kinematic_constraint_set_
const robot_state::RobotState & getCompleteInitialRobotState() const
def get(url)
#define ROS_DEBUG_NAMED(name,...)
ValidConstrainedSampler(const ModelBasedPlanningContext *pc, const kinematic_constraints::KinematicConstraintSetPtr &ks, const constraint_samplers::ConstraintSamplerPtr &cs=constraint_samplers::ConstraintSamplerPtr())
constraint_samplers::ConstraintSamplerPtr constraint_sampler_
virtual bool sample(ompl::base::State *state)
virtual bool project(ompl::base::State *state)
INLINE Rall1d< T, V, S > pow(const Rall1d< T, V, S > &arg, double m)
const ModelBasedStateSpacePtr & getOMPLStateSpace() const


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jul 10 2019 04:03:46