constrained_valid_state_sampler.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/constrained_valid_state_sampler.h>
00038 #include <moveit/ompl_interface/model_based_planning_context.h>
00039 #include <moveit/profiler/profiler.h>
00040 
00041 ompl_interface::ValidConstrainedSampler::ValidConstrainedSampler(
00042     const ModelBasedPlanningContext* pc, const kinematic_constraints::KinematicConstraintSetPtr& ks,
00043     const constraint_samplers::ConstraintSamplerPtr& cs)
00044   : ob::ValidStateSampler(pc->getOMPLSimpleSetup()->getSpaceInformation().get())
00045   , planning_context_(pc)
00046   , kinematic_constraint_set_(ks)
00047   , constraint_sampler_(cs)
00048   , work_state_(pc->getCompleteInitialRobotState())
00049 {
00050   if (!constraint_sampler_)
00051     default_sampler_ = si_->allocStateSampler();
00052   inv_dim_ = si_->getStateSpace()->getDimension() > 0 ? 1.0 / (double)si_->getStateSpace()->getDimension() : 1.0;
00053   logDebug("Constructed a ValidConstrainedSampler instance at address %p", this);
00054 }
00055 
00056 bool ompl_interface::ValidConstrainedSampler::project(ompl::base::State* state)
00057 {
00058   if (constraint_sampler_)
00059   {
00060     planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, state);
00061     if (constraint_sampler_->project(work_state_, planning_context_->getMaximumStateSamplingAttempts()))
00062     {
00063       if (kinematic_constraint_set_->decide(work_state_).satisfied)
00064       {
00065         planning_context_->getOMPLStateSpace()->copyToOMPLState(state, work_state_);
00066         return true;
00067       }
00068     }
00069   }
00070   return false;
00071 }
00072 
00073 bool ompl_interface::ValidConstrainedSampler::sample(ob::State* state)
00074 {
00075   //  moveit::Profiler::ScopedBlock pblock("ValidConstrainedSampler::sample");
00076   if (constraint_sampler_)
00077   {
00078     if (constraint_sampler_->sample(work_state_, planning_context_->getCompleteInitialRobotState(),
00079                                     planning_context_->getMaximumStateSamplingAttempts()))
00080     {
00081       if (kinematic_constraint_set_->decide(work_state_).satisfied)
00082       {
00083         planning_context_->getOMPLStateSpace()->copyToOMPLState(state, work_state_);
00084         return true;
00085       }
00086     }
00087   }
00088   else
00089   {
00090     default_sampler_->sampleUniform(state);
00091     planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, state);
00092     if (kinematic_constraint_set_->decide(work_state_).satisfied)
00093       return true;
00094   }
00095 
00096   return false;
00097 }
00098 
00099 bool ompl_interface::ValidConstrainedSampler::sampleNear(ompl::base::State* state, const ompl::base::State* near,
00100                                                          const double distance)
00101 {
00102   if (!sample(state))
00103     return false;
00104   double total_d = si_->distance(state, near);
00105   if (total_d > distance)
00106   {
00107     double dist = pow(rng_.uniform01(), inv_dim_) * distance;
00108     si_->getStateSpace()->interpolate(near, state, dist / total_d, state);
00109     planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, state);
00110     if (!kinematic_constraint_set_->decide(work_state_).satisfied)
00111       return false;
00112   }
00113   return true;
00114 }


ompl
Author(s): Ioan Sucan
autogenerated on Mon Apr 23 2018 10:25:26