union_constraint_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/constraint_samplers/union_constraint_sampler.h>
00038 #include <moveit/constraint_samplers/default_constraint_samplers.h>
00039 #include <algorithm>
00040 
00041 namespace constraint_samplers
00042 {
00043 struct OrderSamplers
00044 {
00045   bool operator()(const ConstraintSamplerPtr &a, const ConstraintSamplerPtr &b) const
00046   {
00047     const std::vector<std::string> &alinks = a->getJointModelGroup()->getUpdatedLinkModelNames();
00048     const std::vector<std::string> &blinks = b->getJointModelGroup()->getUpdatedLinkModelNames();
00049     std::set<std::string> a_updates(alinks.begin(), alinks.end());
00050     std::set<std::string> b_updates(blinks.begin(), blinks.end());
00051 
00052     bool a_contains_b = std::includes(a_updates.begin(), a_updates.end(),
00053                                       b_updates.begin(), b_updates.end());
00054 
00055     bool b_contains_a = std::includes(b_updates.begin(), b_updates.end(),
00056                                       a_updates.begin(), a_updates.end());
00057 
00058     //a contains b and sets are not equal
00059     if (a_contains_b && !b_contains_a)
00060       return true;
00061     if (b_contains_a && !a_contains_b)
00062       return false;
00063 
00064     //sets are equal or disjoint
00065     bool a_depends_on_b = false;
00066     bool b_depends_on_a = false;
00067     const std::vector<std::string> &fda = a->getFrameDependency();
00068     const std::vector<std::string> &fdb = b->getFrameDependency();
00069     for (std::size_t i = 0 ; i < fda.size() && !a_depends_on_b ; ++i)
00070       for (std::size_t j = 0 ; j < blinks.size() ; ++j)
00071         if (blinks[j] == fda[i])
00072         {
00073           a_depends_on_b = true;
00074           break;
00075         }
00076     for (std::size_t i = 0 ; i < fdb.size() && !b_depends_on_a ; ++i)
00077       for (std::size_t j = 0 ; j < alinks.size() ; ++j)
00078         if (alinks[j] == fdb[i])
00079         {
00080           b_depends_on_a = true;
00081           break;
00082         }
00083     if (b_depends_on_a && a_depends_on_b)
00084     {
00085       logWarn("Circular frame dependency! Sampling will likely produce invalid results (sampling for groups '%s' and '%s')",
00086               a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str());
00087       return true;
00088     }
00089     if (b_depends_on_a && !a_depends_on_b)
00090       return true;
00091     if(a_depends_on_b && !b_depends_on_a)
00092       return false;
00093 
00094     // prefer sampling JointConstraints first
00095     JointConstraintSampler *ja = dynamic_cast<JointConstraintSampler*>(a.get());
00096     JointConstraintSampler *jb = dynamic_cast<JointConstraintSampler*>(b.get());
00097     if (ja && jb == NULL)
00098       return true;
00099     if (jb && ja == NULL)
00100       return false;
00101 
00102     // neither depends on either, so break ties based on group name
00103     return (a->getJointModelGroup()->getName() < b->getJointModelGroup()->getName());
00104   }
00105 };
00106 }
00107 
00108 constraint_samplers::UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name,
00109                                                                     const std::vector<ConstraintSamplerPtr> &samplers) :
00110   ConstraintSampler(scene, group_name), samplers_(samplers)
00111 {
00112   // using stable sort to preserve order of equivalents
00113   std::stable_sort(samplers_.begin(), samplers_.end(), OrderSamplers());
00114 
00115   for (std::size_t i = 0 ; i < samplers_.size() ; ++i)
00116   {
00117     const std::vector<std::string> &fd = samplers_[i]->getFrameDependency();
00118     for (std::size_t j = 0 ; j < fd.size() ; ++j)
00119       frame_depends_.push_back(fd[j]);
00120 
00121     logDebug("Union sampler for group '%s' includes sampler for group '%s'", jmg_->getName().c_str(), samplers_[i]->getJointModelGroup()->getName().c_str());
00122   }
00123 }
00124 
00125 bool constraint_samplers::UnionConstraintSampler::sample(robot_state::RobotState &state, const robot_state::RobotState &reference_state, unsigned int max_attempts)
00126 {
00127   state = reference_state;
00128   state.setToRandomPositions(jmg_);
00129   
00130   if (samplers_.size() >= 1)
00131   {
00132     if (!samplers_[0]->sample(state, reference_state, max_attempts))
00133       return false;
00134   }
00135   
00136   for (std::size_t i = 1 ; i < samplers_.size() ; ++i)
00137     if (!samplers_[i]->sample(state, state, max_attempts))
00138       return false;
00139   
00140   return true;
00141 }
00142 
00143 bool constraint_samplers::UnionConstraintSampler::project(robot_state::RobotState &state, unsigned int max_attempts)
00144 {
00145   for (std::size_t i = 0 ; i < samplers_.size() ; ++i)
00146     if (!samplers_[i]->project(state, max_attempts))
00147       return false;
00148   return true;
00149 }
00150 


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53