state_validity_checker.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/ompl_interface/detail/state_validity_checker.h>
00038 #include <moveit/ompl_interface/model_based_planning_context.h>
00039 #include <moveit/profiler/profiler.h>
00040 #include <ros/ros.h>
00041 
00042 ompl_interface::StateValidityChecker::StateValidityChecker(const ModelBasedPlanningContext* pc)
00043   : ompl::base::StateValidityChecker(pc->getOMPLSimpleSetup()->getSpaceInformation())
00044   , planning_context_(pc)
00045   , group_name_(pc->getGroupName())
00046   , tss_(pc->getCompleteInitialRobotState())
00047   , verbose_(false)
00048 {
00049   specs_.clearanceComputationType = ompl::base::StateValidityCheckerSpecs::APPROXIMATE;
00050   specs_.hasValidDirectionComputation = false;
00051 
00052   collision_request_with_distance_.distance = true;
00053   collision_request_with_cost_.cost = true;
00054 
00055   collision_request_simple_.group_name = planning_context_->getGroupName();
00056   collision_request_with_distance_.group_name = planning_context_->getGroupName();
00057   collision_request_with_cost_.group_name = planning_context_->getGroupName();
00058 
00059   collision_request_simple_verbose_ = collision_request_simple_;
00060   collision_request_simple_verbose_.verbose = true;
00061 
00062   collision_request_with_distance_verbose_ = collision_request_with_distance_;
00063   collision_request_with_distance_verbose_.verbose = true;
00064 }
00065 
00066 void ompl_interface::StateValidityChecker::setVerbose(bool flag)
00067 {
00068   verbose_ = flag;
00069 }
00070 
00071 bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, bool verbose) const
00072 {
00073   //  moveit::Profiler::ScopedBlock sblock("isValid");
00074   return planning_context_->useStateValidityCache() ? isValidWithCache(state, verbose) :
00075                                                       isValidWithoutCache(state, verbose);
00076 }
00077 
00078 bool ompl_interface::StateValidityChecker::isValid(const ompl::base::State* state, double& dist, bool verbose) const
00079 {
00080   //  moveit::Profiler::ScopedBlock sblock("isValid");
00081   return planning_context_->useStateValidityCache() ? isValidWithCache(state, dist, verbose) :
00082                                                       isValidWithoutCache(state, dist, verbose);
00083 }
00084 
00085 double ompl_interface::StateValidityChecker::cost(const ompl::base::State* state) const
00086 {
00087   double cost = 0.0;
00088 
00089   robot_state::RobotState* kstate = tss_.getStateStorage();
00090   planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
00091 
00092   // Calculates cost from a summation of distance to obstacles times the size of the obstacle
00093   collision_detection::CollisionResult res;
00094   planning_context_->getPlanningScene()->checkCollision(collision_request_with_cost_, res, *kstate);
00095 
00096   for (std::set<collision_detection::CostSource>::const_iterator it = res.cost_sources.begin();
00097        it != res.cost_sources.end(); ++it)
00098     cost += it->cost * it->getVolume();
00099 
00100   return cost;
00101 }
00102 
00103 double ompl_interface::StateValidityChecker::clearance(const ompl::base::State* state) const
00104 {
00105   robot_state::RobotState* kstate = tss_.getStateStorage();
00106   planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
00107 
00108   collision_detection::CollisionResult res;
00109   planning_context_->getPlanningScene()->checkCollision(collision_request_with_distance_, res, *kstate);
00110   return res.collision ? 0.0 : (res.distance < 0.0 ? std::numeric_limits<double>::infinity() : res.distance);
00111 }
00112 
00113 bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State* state, bool verbose) const
00114 {
00115   // check bounds
00116   if (!si_->satisfiesBounds(state))
00117   {
00118     if (verbose)
00119       logInform("State outside bounds");
00120     return false;
00121   }
00122 
00123   // convert ompl state to moveit robot state
00124   robot_state::RobotState* kstate = tss_.getStateStorage();
00125   planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
00126 
00127   // check path constraints
00128   const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
00129   if (kset && !kset->decide(*kstate, verbose).satisfied)
00130     return false;
00131 
00132   // check feasibility
00133   if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
00134     return false;
00135 
00136   // check collision avoidance
00137   collision_detection::CollisionResult res;
00138   planning_context_->getPlanningScene()->checkCollision(
00139       verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *kstate);
00140   return res.collision == false;
00141 }
00142 
00143 bool ompl_interface::StateValidityChecker::isValidWithoutCache(const ompl::base::State* state, double& dist,
00144                                                                bool verbose) const
00145 {
00146   if (!si_->satisfiesBounds(state))
00147   {
00148     if (verbose)
00149       logInform("State outside bounds");
00150     return false;
00151   }
00152 
00153   robot_state::RobotState* kstate = tss_.getStateStorage();
00154   planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
00155 
00156   // check path constraints
00157   const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
00158   if (kset)
00159   {
00160     kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*kstate, verbose);
00161     if (!cer.satisfied)
00162     {
00163       dist = cer.distance;
00164       return false;
00165     }
00166   }
00167 
00168   // check feasibility
00169   if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
00170   {
00171     dist = 0.0;
00172     return false;
00173   }
00174 
00175   // check collision avoidance
00176   collision_detection::CollisionResult res;
00177   planning_context_->getPlanningScene()->checkCollision(
00178       verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *kstate);
00179   dist = res.distance;
00180   return res.collision == false;
00181 }
00182 
00183 bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State* state, bool verbose) const
00184 {
00185   if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown())
00186     return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
00187 
00188   if (!si_->satisfiesBounds(state))
00189   {
00190     if (verbose)
00191       logInform("State outside bounds");
00192     const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
00193     return false;
00194   }
00195 
00196   robot_state::RobotState* kstate = tss_.getStateStorage();
00197   planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
00198 
00199   // check path constraints
00200   const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
00201   if (kset && !kset->decide(*kstate, verbose).satisfied)
00202   {
00203     const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
00204     return false;
00205   }
00206 
00207   // check feasibility
00208   if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
00209   {
00210     const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
00211     return false;
00212   }
00213 
00214   // check collision avoidance
00215   collision_detection::CollisionResult res;
00216   planning_context_->getPlanningScene()->checkCollision(
00217       verbose ? collision_request_simple_verbose_ : collision_request_simple_, res, *kstate);
00218   if (res.collision == false)
00219   {
00220     const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markValid();
00221     return true;
00222   }
00223   else
00224   {
00225     const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid();
00226     return false;
00227   }
00228 }
00229 
00230 bool ompl_interface::StateValidityChecker::isValidWithCache(const ompl::base::State* state, double& dist,
00231                                                             bool verbose) const
00232 {
00233   if (state->as<ModelBasedStateSpace::StateType>()->isValidityKnown() &&
00234       state->as<ModelBasedStateSpace::StateType>()->isGoalDistanceKnown())
00235   {
00236     dist = state->as<ModelBasedStateSpace::StateType>()->distance;
00237     return state->as<ModelBasedStateSpace::StateType>()->isMarkedValid();
00238   }
00239 
00240   if (!si_->satisfiesBounds(state))
00241   {
00242     if (verbose)
00243       logInform("State outside bounds");
00244     const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(0.0);
00245     return false;
00246   }
00247 
00248   robot_state::RobotState* kstate = tss_.getStateStorage();
00249   planning_context_->getOMPLStateSpace()->copyToRobotState(*kstate, state);
00250 
00251   // check path constraints
00252   const kinematic_constraints::KinematicConstraintSetPtr& kset = planning_context_->getPathConstraints();
00253   if (kset)
00254   {
00255     kinematic_constraints::ConstraintEvaluationResult cer = kset->decide(*kstate, verbose);
00256     if (!cer.satisfied)
00257     {
00258       dist = cer.distance;
00259       const_cast<ob::State*>(state)->as<ModelBasedStateSpace::StateType>()->markInvalid(dist);
00260       return false;
00261     }
00262   }
00263 
00264   // check feasibility
00265   if (!planning_context_->getPlanningScene()->isStateFeasible(*kstate, verbose))
00266   {
00267     dist = 0.0;
00268     return false;
00269   }
00270 
00271   // check collision avoidance
00272   collision_detection::CollisionResult res;
00273   planning_context_->getPlanningScene()->checkCollision(
00274       verbose ? collision_request_with_distance_verbose_ : collision_request_with_distance_, res, *kstate);
00275   dist = res.distance;
00276   return res.collision == false;
00277 }


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:27