constrained_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_sampler.h>
00038 #include <moveit/ompl_interface/model_based_planning_context.h>
00039 #include <moveit/profiler/profiler.h>
00040 
00041 ompl_interface::ConstrainedSampler::ConstrainedSampler(const ModelBasedPlanningContext* pc,
00042                                                        const constraint_samplers::ConstraintSamplerPtr& cs)
00043   : ob::StateSampler(pc->getOMPLStateSpace().get())
00044   , planning_context_(pc)
00045   , default_(space_->allocDefaultStateSampler())
00046   , constraint_sampler_(cs)
00047   , work_state_(pc->getCompleteInitialRobotState())
00048   , constrained_success_(0)
00049   , constrained_failure_(0)
00050 {
00051   inv_dim_ = space_->getDimension() > 0 ? 1.0 / (double)space_->getDimension() : 1.0;
00052 }
00053 
00054 double ompl_interface::ConstrainedSampler::getConstrainedSamplingRate() const
00055 {
00056   if (constrained_success_ == 0)
00057     return 0.0;
00058   else
00059     return (double)constrained_success_ / (double)(constrained_success_ + constrained_failure_);
00060 }
00061 
00062 bool ompl_interface::ConstrainedSampler::sampleC(ob::State* state)
00063 {
00064   //  moveit::Profiler::ScopedBlock sblock("sampleWithConstraints");
00065 
00066   if (constraint_sampler_->sample(work_state_, planning_context_->getCompleteInitialRobotState(),
00067                                   planning_context_->getMaximumStateSamplingAttempts()))
00068   {
00069     planning_context_->getOMPLStateSpace()->copyToOMPLState(state, work_state_);
00070     if (space_->satisfiesBounds(state))
00071     {
00072       ++constrained_success_;
00073       return true;
00074     }
00075   }
00076   ++constrained_failure_;
00077   return false;
00078 }
00079 
00080 void ompl_interface::ConstrainedSampler::sampleUniform(ob::State* state)
00081 {
00082   if (!sampleC(state) && !sampleC(state) && !sampleC(state))
00083     default_->sampleUniform(state);
00084 }
00085 
00086 void ompl_interface::ConstrainedSampler::sampleUniformNear(ob::State* state, const ob::State* near,
00087                                                            const double distance)
00088 {
00089   if (sampleC(state) || sampleC(state) || sampleC(state))
00090   {
00091     double total_d = space_->distance(state, near);
00092     if (total_d > distance)
00093     {
00094       double dist = pow(rng_.uniform01(), inv_dim_) * distance;
00095       space_->interpolate(near, state, dist / total_d, state);
00096     }
00097   }
00098   else
00099     default_->sampleUniformNear(state, near, distance);
00100 }
00101 
00102 void ompl_interface::ConstrainedSampler::sampleGaussian(ob::State* state, const ob::State* mean, const double stdDev)
00103 {
00104   if (sampleC(state) || sampleC(state) || sampleC(state))
00105   {
00106     double total_d = space_->distance(state, mean);
00107     double distance = rng_.gaussian(0.0, stdDev);
00108     if (total_d > distance)
00109     {
00110       double dist = pow(rng_.uniform01(), inv_dim_) * distance;
00111       space_->interpolate(mean, state, dist / total_d, state);
00112     }
00113   }
00114   else
00115     default_->sampleGaussian(state, mean, stdDev);
00116 }


ompl
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:33