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(), b_updates.begin(), b_updates.end());
00053 
00054     bool b_contains_a = std::includes(b_updates.begin(), b_updates.end(), a_updates.begin(), a_updates.end());
00055 
00056     // a contains b and sets are not equal
00057     if (a_contains_b && !b_contains_a)
00058       return true;
00059     if (b_contains_a && !a_contains_b)
00060       return false;
00061 
00062     // sets are equal or disjoint
00063     bool a_depends_on_b = false;
00064     bool b_depends_on_a = false;
00065     const std::vector<std::string>& fda = a->getFrameDependency();
00066     const std::vector<std::string>& fdb = b->getFrameDependency();
00067     for (std::size_t i = 0; i < fda.size() && !a_depends_on_b; ++i)
00068       for (std::size_t j = 0; j < blinks.size(); ++j)
00069         if (blinks[j] == fda[i])
00070         {
00071           a_depends_on_b = true;
00072           break;
00073         }
00074     for (std::size_t i = 0; i < fdb.size() && !b_depends_on_a; ++i)
00075       for (std::size_t j = 0; j < alinks.size(); ++j)
00076         if (alinks[j] == fdb[i])
00077         {
00078           b_depends_on_a = true;
00079           break;
00080         }
00081     if (b_depends_on_a && a_depends_on_b)
00082     {
00083       logWarn("Circular frame dependency! Sampling will likely produce invalid results (sampling for groups '%s' and "
00084               "'%s')",
00085               a->getJointModelGroup()->getName().c_str(), b->getJointModelGroup()->getName().c_str());
00086       return true;
00087     }
00088     if (b_depends_on_a && !a_depends_on_b)
00089       return true;
00090     if (a_depends_on_b && !b_depends_on_a)
00091       return false;
00092 
00093     // prefer sampling JointConstraints first
00094     JointConstraintSampler* ja = dynamic_cast<JointConstraintSampler*>(a.get());
00095     JointConstraintSampler* jb = dynamic_cast<JointConstraintSampler*>(b.get());
00096     if (ja && jb == NULL)
00097       return true;
00098     if (jb && ja == NULL)
00099       return false;
00100 
00101     // neither depends on either, so break ties based on group name
00102     return (a->getJointModelGroup()->getName() < b->getJointModelGroup()->getName());
00103   }
00104 };
00105 }
00106 
00107 constraint_samplers::UnionConstraintSampler::UnionConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene,
00108                                                                     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(),
00122              samplers_[i]->getJointModelGroup()->getName().c_str());
00123   }
00124 }
00125 
00126 bool constraint_samplers::UnionConstraintSampler::sample(robot_state::RobotState& state,
00127                                                          const robot_state::RobotState& reference_state,
00128                                                          unsigned int max_attempts)
00129 {
00130   state = reference_state;
00131   state.setToRandomPositions(jmg_);
00132 
00133   if (samplers_.size() >= 1)
00134   {
00135     if (!samplers_[0]->sample(state, reference_state, max_attempts))
00136       return false;
00137   }
00138 
00139   for (std::size_t i = 1; i < samplers_.size(); ++i)
00140   {
00141     // ConstraintSampler::sample returns states with dirty link transforms (because it only writes values)
00142     // but requires a state with clean link transforms as input. This means that we need to clean the link
00143     // transforms between calls to ConstraintSampler::sample.
00144     state.updateLinkTransforms();
00145     if (!samplers_[i]->sample(state, state, max_attempts))
00146       return false;
00147   }
00148   return true;
00149 }
00150 
00151 bool constraint_samplers::UnionConstraintSampler::project(robot_state::RobotState& state, unsigned int max_attempts)
00152 {
00153   for (std::size_t i = 0; i < samplers_.size(); ++i)
00154   {
00155     // ConstraintSampler::project returns states with dirty link transforms (because it only writes values)
00156     // but requires a state with clean link transforms as input. This means that we need to clean the link
00157     // transforms between calls to ConstraintSampler::sample.
00158     state.updateLinkTransforms();
00159     if (!samplers_[i]->project(state, max_attempts))
00160       return false;
00161   }
00162   return true;
00163 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Apr 23 2018 10:24:45