constrained_goal_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_goal_sampler.h>
00038 #include <moveit/ompl_interface/model_based_planning_context.h>
00039 #include <moveit/ompl_interface/detail/state_validity_checker.h>
00040 #include <moveit/profiler/profiler.h>
00041 
00042 ompl_interface::ConstrainedGoalSampler::ConstrainedGoalSampler(const ModelBasedPlanningContext *pc,
00043                                                                const kinematic_constraints::KinematicConstraintSetPtr &ks,
00044                                                                const constraint_samplers::ConstraintSamplerPtr &cs) 
00045   : ob::GoalLazySamples(pc->getOMPLSimpleSetup()->getSpaceInformation(),
00046                         boost::bind(&ConstrainedGoalSampler::sampleUsingConstraintSampler, this, _1, _2), false)
00047   , planning_context_(pc)
00048   , kinematic_constraint_set_(ks)
00049   , constraint_sampler_(cs)
00050   , work_state_(pc->getCompleteInitialRobotState())
00051   , invalid_sampled_constraints_(0)
00052   , warned_invalid_samples_(false)
00053   , verbose_display_(0)
00054 {
00055   if (!constraint_sampler_)
00056     default_sampler_ = si_->allocStateSampler();
00057   logDebug("Constructed a ConstrainedGoalSampler instance at address %p", this);
00058   startSampling();
00059 }
00060 
00061 bool ompl_interface::ConstrainedGoalSampler::checkStateValidity(ob::State* new_goal,
00062                                                                 const robot_state::RobotState& state,
00063                                                                 bool verbose) const
00064 {
00065   planning_context_->getOMPLStateSpace()->copyToOMPLState(new_goal, state);
00066   return static_cast<const StateValidityChecker*>(si_->getStateValidityChecker().get())->isValid(new_goal, verbose);
00067 }
00068 
00069 bool ompl_interface::ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, 
00070                                                                    robot_state::RobotState const* state,
00071                                                                    const robot_model::JointModelGroup* jmg,
00072                                                                    const double* jpos,
00073                                                                    bool verbose) const
00074 {
00075   // we copy the state to not change the seed state
00076   robot_state::RobotState solution_state( *state );
00077   solution_state.setJointGroupPositions(jmg, jpos);
00078   solution_state.update();
00079   return checkStateValidity(new_goal, solution_state, verbose);
00080 }
00081 
00082 bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const ob::GoalLazySamples *gls, ob::State *new_goal)
00083 {
00084   //  moveit::Profiler::ScopedBlock sblock("ConstrainedGoalSampler::sampleUsingConstraintSampler");
00085   
00086   unsigned int max_attempts = planning_context_->getMaximumGoalSamplingAttempts();
00087   unsigned int attempts_so_far = gls->samplingAttemptsCount();
00088   
00089   // terminate after too many attempts
00090   if (attempts_so_far >= max_attempts)
00091     return false;
00092   
00093   // terminate after a maximum number of samples
00094   if (gls->getStateCount() >= planning_context_->getMaximumGoalSamples())
00095     return false;
00096   
00097   // terminate the sampling thread when a solution has been found
00098   if (planning_context_->getOMPLSimpleSetup()->getProblemDefinition()->hasSolution())
00099     return false;
00100 
00101   unsigned int max_attempts_div2 = max_attempts/2;
00102   for (unsigned int a = gls->samplingAttemptsCount() ; a < max_attempts && gls->isSampling() ; ++a)
00103   {
00104     bool verbose = false;
00105     if (gls->getStateCount() == 0 && a >= max_attempts_div2)
00106       if (verbose_display_ < 1)
00107       {
00108         verbose = true;
00109         verbose_display_++;
00110       }
00111     
00112     if (constraint_sampler_)
00113     {
00114       // makes the constraint sampler also perform a validity callback
00115       robot_state::GroupStateValidityCallbackFn gsvcf = boost::bind(&ompl_interface::ConstrainedGoalSampler::stateValidityCallback,
00116                                                                     this,
00117                                                                     new_goal, 
00118                                                                     _1,  // pointer to state 
00119                                                                     _2,  // const* joint model group
00120                                                                     _3,  // double* of joint positions
00121                                                                     verbose);
00122       constraint_sampler_->setGroupStateValidityCallback( gsvcf );
00123 
00124       if (constraint_sampler_->project(work_state_, planning_context_->getMaximumStateSamplingAttempts()))
00125       {
00126         work_state_.update();
00127         if (kinematic_constraint_set_->decide(work_state_, verbose).satisfied)
00128         {
00129           if (checkStateValidity(new_goal, work_state_, verbose))
00130             return true;
00131         }
00132         else
00133         {
00134           invalid_sampled_constraints_++;
00135           if (!warned_invalid_samples_ && invalid_sampled_constraints_ >= (attempts_so_far * 8) / 10)
00136           {
00137             warned_invalid_samples_ = true;
00138             logWarn("More than 80%% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?");
00139           }
00140         }
00141       }
00142     }
00143     else
00144     {
00145       default_sampler_->sampleUniform(new_goal);
00146       if (static_cast<const StateValidityChecker*>(si_->getStateValidityChecker().get())->isValid(new_goal, verbose))
00147       {
00148         planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, new_goal);
00149         if (kinematic_constraint_set_->decide(work_state_, verbose).satisfied)
00150           return true;
00151       }
00152     }
00153   }
00154   return false;
00155 }


ompl
Author(s): Ioan Sucan
autogenerated on Wed Sep 16 2015 04:42:27