constrained_goal_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 
41 
43  const ModelBasedPlanningContext* pc, const kinematic_constraints::KinematicConstraintSetPtr& ks,
44  const constraint_samplers::ConstraintSamplerPtr& cs)
45  : ob::GoalLazySamples(pc->getOMPLSimpleSetup()->getSpaceInformation(),
46  boost::bind(&ConstrainedGoalSampler::sampleUsingConstraintSampler, this, _1, _2), false)
47  , planning_context_(pc)
48  , kinematic_constraint_set_(ks)
49  , constraint_sampler_(cs)
50  , work_state_(pc->getCompleteInitialRobotState())
51  , invalid_sampled_constraints_(0)
52  , warned_invalid_samples_(false)
53  , verbose_display_(0)
54 {
56  default_sampler_ = si_->allocStateSampler();
57  ROS_DEBUG_NAMED("constrained_goal_sampler", "Constructed a ConstrainedGoalSampler instance at address %p", this);
58  startSampling();
59 }
60 
62  const robot_state::RobotState& state,
63  bool verbose) const
64 {
65  planning_context_->getOMPLStateSpace()->copyToOMPLState(new_goal, state);
66  return static_cast<const StateValidityChecker*>(si_->getStateValidityChecker().get())->isValid(new_goal, verbose);
67 }
68 
70  robot_state::RobotState const* state,
71  const robot_model::JointModelGroup* jmg,
72  const double* jpos, bool verbose) const
73 {
74  // we copy the state to not change the seed state
75  robot_state::RobotState solution_state(*state);
76  solution_state.setJointGroupPositions(jmg, jpos);
77  solution_state.update();
78  return checkStateValidity(new_goal, solution_state, verbose);
79 }
80 
82  ob::State* new_goal)
83 {
84  // moveit::Profiler::ScopedBlock sblock("ConstrainedGoalSampler::sampleUsingConstraintSampler");
85 
86  unsigned int max_attempts = planning_context_->getMaximumGoalSamplingAttempts();
87  unsigned int attempts_so_far = gls->samplingAttemptsCount();
88 
89  // terminate after too many attempts
90  if (attempts_so_far >= max_attempts)
91  return false;
92 
93  // terminate after a maximum number of samples
94  if (gls->getStateCount() >= planning_context_->getMaximumGoalSamples())
95  return false;
96 
97  // terminate the sampling thread when a solution has been found
98  if (planning_context_->getOMPLSimpleSetup()->getProblemDefinition()->hasSolution())
99  return false;
100 
101  unsigned int max_attempts_div2 = max_attempts / 2;
102  for (unsigned int a = gls->samplingAttemptsCount(); a < max_attempts && gls->isSampling(); ++a)
103  {
104  bool verbose = false;
105  if (gls->getStateCount() == 0 && a >= max_attempts_div2)
106  if (verbose_display_ < 1)
107  {
108  verbose = true;
110  }
111 
113  {
114  // makes the constraint sampler also perform a validity callback
115  robot_state::GroupStateValidityCallbackFn gsvcf =
117  _1, // pointer to state
118  _2, // const* joint model group
119  _3, // double* of joint positions
120  verbose);
121  constraint_sampler_->setGroupStateValidityCallback(gsvcf);
122 
124  {
125  work_state_.update();
126  if (kinematic_constraint_set_->decide(work_state_, verbose).satisfied)
127  {
128  if (checkStateValidity(new_goal, work_state_, verbose))
129  return true;
130  }
131  else
132  {
134  if (!warned_invalid_samples_ && invalid_sampled_constraints_ >= (attempts_so_far * 8) / 10)
135  {
137  ROS_WARN_NAMED("constrained_goal_sampler", "More than 80%% of the sampled goal states "
138  "fail to satisfy the constraints imposed on the goal sampler. "
139  "Is the constrained sampler working correctly?");
140  }
141  }
142  }
143  }
144  else
145  {
146  default_sampler_->sampleUniform(new_goal);
147  if (static_cast<const StateValidityChecker*>(si_->getStateValidityChecker().get())->isValid(new_goal, verbose))
148  {
149  planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, new_goal);
150  if (kinematic_constraint_set_->decide(work_state_, verbose).satisfied)
151  return true;
152  }
153  }
154  }
155  return false;
156 }
bool stateValidityCallback(ompl::base::State *new_goal, robot_state::RobotState const *state, const robot_model::JointModelGroup *, const double *, bool verbose=false) const
const ModelBasedPlanningContext * planning_context_
#define ROS_WARN_NAMED(name,...)
const og::SimpleSetupPtr & getOMPLSimpleSetup() const
#define ROS_DEBUG_NAMED(name,...)
bool verbose
ConstrainedGoalSampler(const ModelBasedPlanningContext *pc, const kinematic_constraints::KinematicConstraintSetPtr &ks, const constraint_samplers::ConstraintSamplerPtr &cs=constraint_samplers::ConstraintSamplerPtr())
constraint_samplers::ConstraintSamplerPtr constraint_sampler_
bool checkStateValidity(ompl::base::State *new_goal, const robot_state::RobotState &state, bool verbose=false) const
An interface for a OMPL state validity checker.
bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples *gls, ompl::base::State *new_goal)
kinematic_constraints::KinematicConstraintSetPtr kinematic_constraint_set_
const ModelBasedStateSpacePtr & getOMPLStateSpace() const


ompl
Author(s): Ioan Sucan
autogenerated on Sun Oct 21 2018 10:37:13