state_validity_checker.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 #include <ros/ros.h>
41 
43  : ompl::base::StateValidityChecker(pc->getOMPLSimpleSetup()->getSpaceInformation())
44  , planning_context_(pc)
45  , group_name_(pc->getGroupName())
46  , tss_(pc->getCompleteInitialRobotState())
47  , verbose_(false)
48 {
49  specs_.clearanceComputationType = ompl::base::StateValidityCheckerSpecs::APPROXIMATE;
50  specs_.hasValidDirectionComputation = false;
51 
54 
58 
61 
64 }
65 
67 {
68  verbose_ = flag;
69 }
70 
71 bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, bool verbose) const
72 {
73  // moveit::Profiler::ScopedBlock sblock("isValid");
74  return planning_context_->useStateValidityCache() ? isValidWithCache(state, verbose) :
75  isValidWithoutCache(state, verbose);
76 }
77 
78 bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, double& dist, bool verbose) const
79 {
80  // moveit::Profiler::ScopedBlock sblock("isValid");
81  return planning_context_->useStateValidityCache() ? isValidWithCache(state, dist, verbose) :
82  isValidWithoutCache(state, dist, verbose);
83 }
84 
85 double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state) const
86 {
87  double cost = 0.0;
88 
89  robot_state::RobotState* kstate = tss_.getStateStorage();
90  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
91 
92  // Calculates cost from a summation of distance to obstacles times the size of the obstacle
94  planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *kstate);
95 
96  for (std::set<collision_detection::CostSource>::const_iterator it = res.cost_sources.begin();
97  it != res.cost_sources.end(); ++it)
98  cost += it->cost * it->getVolume();
99 
100  return cost;
101 }
102 
103 double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* state) const
104 {
105  robot_state::RobotState* kstate = tss_.getStateStorage();
106  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
107 
109  planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *kstate);
110  return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits<double>::infinity() : res.distance);
111 }
112 
113 bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State* state, bool verbose) const
114 {
115  // check bounds
116  if (!si_->satisfiesBounds(state))
117  {
118  if (verbose)
119  ROS_INFO_NAMED("state_validity_checker", "State outside bounds");
120  return false;
121  }
122 
123  // convert ompl state to moveit robot state
124  robot_state::RobotState* kstate = tss_.getStateStorage();
125  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
126 
127  // check path constraints
128  const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
129  if (kset && !kset->decide(*kstate, verbose).satisfied)
130  return false;
131 
132  // check feasibility
133  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
134  return false;
135 
136  // check collision avoidance
138  planning_context_->getPlanningScene()->checkCollision(
140  return res.collision == false;
141 }
142 
143 bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State* state, double& dist,
144  bool verbose) const
145 {
146  if (!si_->satisfiesBounds(state))
147  {
148  if (verbose)
149  ROS_INFO_NAMED("state_validity_checker", "State outside bounds");
150  return false;
151  }
152 
153  robot_state::RobotState* kstate = tss_.getStateStorage();
154  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
155 
156  // check path constraints
157  const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
158  if (kset)
159  {
160  kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*kstate, verbose);
161  if (!cer.satisfied)
162  {
163  dist = cer.distance;
164  return false;
165  }
166  }
167 
168  // check feasibility
169  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
170  {
171  dist = 0.0;
172  return false;
173  }
174 
175  // check collision avoidance
177  planning_context_->getPlanningScene()->checkCollision(
179  dist = res.distance;
180  return res.collision == false;
181 }
182 
183 bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State* state, bool verbose) const
184 {
186  return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
187 
188  if (!si_->satisfiesBounds(state))
189  {
190  if (verbose)
191  ROS_INFO_NAMED("state_validity_checker", "State outside bounds");
192  const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
193  return false;
194  }
195 
196  robot_state::RobotState* kstate = tss_.getStateStorage();
197  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
198 
199  // check path constraints
200  const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
201  if (kset && !kset->decide(*kstate, verbose).satisfied)
202  {
203  const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
204  return false;
205  }
206 
207  // check feasibility
208  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
209  {
210  const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
211  return false;
212  }
213 
214  // check collision avoidance
216  planning_context_->getPlanningScene()->checkCollision(
218  if (res.collision == false)
219  {
220  const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markValid();
221  return true;
222  }
223  else
224  {
225  const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
226  return false;
227  }
228 }
229 
230 bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State* state, double& dist,
231  bool verbose) const
232 {
233  if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown() &&
234  state->as<ModelBasedStateSpace::StateType>()->isGoalDistanceKnown())
235  {
236  dist = state->as<ModelBasedStateSpace::StateType>()->distance;
237  return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
238  }
239 
240  if (!si_->satisfiesBounds(state))
241  {
242  if (verbose)
243  ROS_INFO_NAMED("state_validity_checker", "State outside bounds");
244  const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(0.0);
245  return false;
246  }
247 
248  robot_state::RobotState* kstate = tss_.getStateStorage();
249  planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
250 
251  // check path constraints
252  const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
253  if (kset)
254  {
255  kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*kstate, verbose);
256  if (!cer.satisfied)
257  {
258  dist = cer.distance;
259  const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(dist);
260  return false;
261  }
262  }
263 
264  // check feasibility
265  if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
266  {
267  dist = 0.0;
268  return false;
269  }
270 
271  // check collision avoidance
273  planning_context_->getPlanningScene()->checkCollision(
275  dist = res.distance;
276  return res.collision == false;
277 }
collision_detection::CollisionRequest collision_request_simple_verbose_
collision_detection::CollisionRequest collision_request_with_cost_
#define ROS_INFO_NAMED(name,...)
bool isValidWithCache(const ompl::base::State *state, bool verbose) const
collision_detection::CollisionRequest collision_request_simple_
virtual double cost(const ompl::base::State *state) const
collision_detection::CollisionRequest collision_request_with_distance_
StateValidityChecker(const ModelBasedPlanningContext *planning_context)
const planning_scene::PlanningSceneConstPtr & getPlanningScene() const
robot_state::RobotState * getStateStorage() const
const ModelBasedPlanningContext * planning_context_
std::set< CostSource > cost_sources
bool isValidWithoutCache(const ompl::base::State *state, bool verbose) const
const kinematic_constraints::KinematicConstraintSetPtr & getPathConstraints() const
virtual double clearance(const ompl::base::State *state) const
const std::string & getGroupName() const
collision_detection::CollisionRequest collision_request_with_distance_verbose_
An interface for a OMPL state validity checker.
double distance(const urdf::Pose &transform)
virtual bool isValid(const ompl::base::State *state) const
const ModelBasedStateSpacePtr & getOMPLStateSpace() const


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