constraint_sampler_manager.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/constraint_sampler_manager.h>
00038 #include <moveit/constraint_samplers/default_constraint_samplers.h>
00039 #include <moveit/constraint_samplers/union_constraint_sampler.h>
00040 #include <sstream>
00041 
00042 constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectSampler(
00043     const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name,
00044     const moveit_msgs::Constraints& constr) const
00045 {
00046   for (std::size_t i = 0; i < sampler_alloc_.size(); ++i)
00047     if (sampler_alloc_[i]->canService(scene, group_name, constr))
00048       return sampler_alloc_[i]->alloc(scene, group_name, constr);
00049 
00050   // if no default sampler was used, try a default one
00051   return selectDefaultSampler(scene, group_name, constr);
00052 }
00053 
00054 constraint_samplers::ConstraintSamplerPtr constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(
00055     const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name,
00056     const moveit_msgs::Constraints& constr)
00057 {
00058   const robot_model::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group_name);
00059   if (!jmg)
00060     return constraint_samplers::ConstraintSamplerPtr();
00061   std::stringstream ss;
00062   ss << constr;
00063   logDebug("Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n",
00064            jmg->getName().c_str(), ss.str().c_str());
00065 
00066   ConstraintSamplerPtr joint_sampler;  // location to put chosen joint sampler if needed
00067   // if there are joint constraints, we could possibly get a sampler from those
00068   if (!constr.joint_constraints.empty())
00069   {
00070     logDebug("There are joint constraints specified. Attempting to construct a JointConstraintSampler for group '%s'",
00071              jmg->getName().c_str());
00072 
00073     std::map<std::string, bool> joint_coverage;
00074     for (std::size_t i = 0; i < jmg->getVariableNames().size(); ++i)
00075       joint_coverage[jmg->getVariableNames()[i]] = false;
00076 
00077     // construct the constraints
00078     std::vector<kinematic_constraints::JointConstraint> jc;
00079     for (std::size_t i = 0; i < constr.joint_constraints.size(); ++i)
00080     {
00081       kinematic_constraints::JointConstraint j(scene->getRobotModel());
00082       if (j.configure(constr.joint_constraints[i]))
00083       {
00084         if (joint_coverage.find(j.getJointVariableName()) != joint_coverage.end())
00085         {
00086           joint_coverage[j.getJointVariableName()] = true;
00087           jc.push_back(j);
00088         }
00089       }
00090     }
00091 
00092     // check if every joint is covered (constrained) by just joint samplers
00093     bool full_coverage = true;
00094     for (std::map<std::string, bool>::iterator it = joint_coverage.begin(); it != joint_coverage.end(); ++it)
00095       if (!it->second)
00096       {
00097         full_coverage = false;
00098         break;
00099       }
00100 
00101     // if we have constrained every joint, then we just use a sampler using these constraints
00102     if (full_coverage)
00103     {
00104       JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
00105       if (sampler->configure(jc))
00106       {
00107         logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
00108         return sampler;
00109       }
00110     }
00111     else
00112         // if a smaller set of joints has been specified, keep the constraint sampler around, but use it only if no IK
00113         // sampler has been specified.
00114         if (!jc.empty())
00115     {
00116       JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
00117       if (sampler->configure(jc))
00118       {
00119         logDebug("Temporary sampler satisfying joint constraints for group '%s' allocated. Looking for different types "
00120                  "of constraints before returning though.",
00121                  jmg->getName().c_str());
00122         joint_sampler = sampler;
00123       }
00124     }
00125   }
00126 
00127   std::vector<ConstraintSamplerPtr> samplers;
00128   if (joint_sampler)  // Start making a union of constraint samplers
00129     samplers.push_back(joint_sampler);
00130 
00131   // read the ik allocators, if any
00132   const robot_model::JointModelGroup::KinematicsSolver& ik_alloc = jmg->getGroupKinematics().first;
00133   const robot_model::JointModelGroup::KinematicsSolverMap& ik_subgroup_alloc = jmg->getGroupKinematics().second;
00134 
00135   // if we have a means of computing complete states for the group using IK, then we try to see if any IK constraints
00136   // should be used
00137   if (ik_alloc)
00138   {
00139     logDebug("There is an IK allocator for '%s'. Checking for corresponding position and/or orientation constraints",
00140              jmg->getName().c_str());
00141 
00142     // keep track of which links we constrained
00143     std::map<std::string, IKConstraintSamplerPtr> usedL;
00144 
00145     // if we have position and/or orientation constraints on links that we can perform IK for,
00146     // we will use a sampleable goal region that employs IK to sample goals;
00147     // if there are multiple constraints for the same link, we keep the one with the smallest
00148     // volume for sampling
00149     for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
00150       for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
00151         if (constr.position_constraints[p].link_name == constr.orientation_constraints[o].link_name)
00152         {
00153           kinematic_constraints::PositionConstraintPtr pc(
00154               new kinematic_constraints::PositionConstraint(scene->getRobotModel()));
00155           kinematic_constraints::OrientationConstraintPtr oc(
00156               new kinematic_constraints::OrientationConstraint(scene->getRobotModel()));
00157           if (pc->configure(constr.position_constraints[p], scene->getTransforms()) &&
00158               oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
00159           {
00160             IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
00161             if (iks->configure(IKSamplingPose(pc, oc)))
00162             {
00163               bool use = true;
00164               // Check if there already is a constraint on this link
00165               if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
00166                 // If there is, check if the previous one has a smaller volume for sampling
00167                 if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
00168                   use = false;  // only use new constraint if it has a smaller sampling volume
00169               if (use)
00170               {
00171                 // assign the link to a new constraint sampler
00172                 usedL[constr.position_constraints[p].link_name] = iks;
00173                 logDebug("Allocated an IK-based sampler for group '%s' satisfying position and orientation constraints "
00174                          "on link '%s'",
00175                          jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
00176               }
00177             }
00178           }
00179         }
00180 
00181     // keep track of links constrained with a full pose
00182     std::map<std::string, IKConstraintSamplerPtr> usedL_fullPose = usedL;
00183 
00184     for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
00185     {
00186       // if we are constraining this link with a full pose, we do not attempt to constrain it with a position constraint
00187       // only
00188       if (usedL_fullPose.find(constr.position_constraints[p].link_name) != usedL_fullPose.end())
00189         continue;
00190 
00191       kinematic_constraints::PositionConstraintPtr pc(
00192           new kinematic_constraints::PositionConstraint(scene->getRobotModel()));
00193       if (pc->configure(constr.position_constraints[p], scene->getTransforms()))
00194       {
00195         IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
00196         if (iks->configure(IKSamplingPose(pc)))
00197         {
00198           bool use = true;
00199           if (usedL.find(constr.position_constraints[p].link_name) != usedL.end())
00200             if (usedL[constr.position_constraints[p].link_name]->getSamplingVolume() < iks->getSamplingVolume())
00201               use = false;
00202           if (use)
00203           {
00204             usedL[constr.position_constraints[p].link_name] = iks;
00205             logDebug("Allocated an IK-based sampler for group '%s' satisfying position constraints on link '%s'",
00206                      jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
00207           }
00208         }
00209       }
00210     }
00211 
00212     for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
00213     {
00214       // if we are constraining this link with a full pose, we do not attempt to constrain it with an orientation
00215       // constraint only
00216       if (usedL_fullPose.find(constr.orientation_constraints[o].link_name) != usedL_fullPose.end())
00217         continue;
00218 
00219       kinematic_constraints::OrientationConstraintPtr oc(
00220           new kinematic_constraints::OrientationConstraint(scene->getRobotModel()));
00221       if (oc->configure(constr.orientation_constraints[o], scene->getTransforms()))
00222       {
00223         IKConstraintSamplerPtr iks(new IKConstraintSampler(scene, jmg->getName()));
00224         if (iks->configure(IKSamplingPose(oc)))
00225         {
00226           bool use = true;
00227           if (usedL.find(constr.orientation_constraints[o].link_name) != usedL.end())
00228             if (usedL[constr.orientation_constraints[o].link_name]->getSamplingVolume() < iks->getSamplingVolume())
00229               use = false;
00230           if (use)
00231           {
00232             usedL[constr.orientation_constraints[o].link_name] = iks;
00233             logDebug("Allocated an IK-based sampler for group '%s' satisfying orientation constraints on link '%s'",
00234                      jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str());
00235           }
00236         }
00237       }
00238     }
00239 
00240     if (usedL.size() == 1)
00241     {
00242       if (samplers.empty())
00243         return usedL.begin()->second;
00244       else
00245       {
00246         samplers.push_back(usedL.begin()->second);
00247         return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
00248       }
00249     }
00250     else if (usedL.size() > 1)
00251     {
00252       logDebug("Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume",
00253                jmg->getName().c_str());
00254       // find the sampler with the smallest sampling volume; delete the rest
00255       IKConstraintSamplerPtr iks = usedL.begin()->second;
00256       double msv = iks->getSamplingVolume();
00257       for (std::map<std::string, IKConstraintSamplerPtr>::const_iterator it = ++usedL.begin(); it != usedL.end(); ++it)
00258       {
00259         double v = it->second->getSamplingVolume();
00260         if (v < msv)
00261         {
00262           iks = it->second;
00263           msv = v;
00264         }
00265       }
00266       if (samplers.empty())
00267       {
00268         return iks;
00269       }
00270       else
00271       {
00272         samplers.push_back(iks);
00273         return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
00274       }
00275     }
00276   }
00277 
00278   // if we got to this point, we have not decided on a sampler.
00279   // we now check to see if we can use samplers from subgroups
00280   if (!ik_subgroup_alloc.empty())
00281   {
00282     logDebug("There are IK allocators for subgroups of group '%s'. Checking for corresponding position and/or "
00283              "orientation constraints",
00284              jmg->getName().c_str());
00285 
00286     bool some_sampler_valid = false;
00287 
00288     std::set<std::size_t> usedP, usedO;
00289     for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = ik_subgroup_alloc.begin();
00290          it != ik_subgroup_alloc.end(); ++it)
00291     {
00292       // construct a sub-set of constraints that operate on the sub-group for which we have an IK allocator
00293       moveit_msgs::Constraints sub_constr;
00294       for (std::size_t p = 0; p < constr.position_constraints.size(); ++p)
00295         if (it->first->hasLinkModel(constr.position_constraints[p].link_name))
00296           if (usedP.find(p) == usedP.end())
00297           {
00298             sub_constr.position_constraints.push_back(constr.position_constraints[p]);
00299             usedP.insert(p);
00300           }
00301 
00302       for (std::size_t o = 0; o < constr.orientation_constraints.size(); ++o)
00303         if (it->first->hasLinkModel(constr.orientation_constraints[o].link_name))
00304           if (usedO.find(o) == usedO.end())
00305           {
00306             sub_constr.orientation_constraints.push_back(constr.orientation_constraints[o]);
00307             usedO.insert(o);
00308           }
00309 
00310       // if some matching constraints were found, construct the allocator
00311       if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
00312       {
00313         logDebug("Attempting to construct a sampler for the '%s' subgroup of '%s'", it->first->getName().c_str(),
00314                  jmg->getName().c_str());
00315         ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr);
00316         if (cs)
00317         {
00318           logDebug("Constructed a sampler for the joints corresponding to group '%s', but part of group '%s'",
00319                    it->first->getName().c_str(), jmg->getName().c_str());
00320           some_sampler_valid = true;
00321           samplers.push_back(cs);
00322         }
00323       }
00324     }
00325     if (some_sampler_valid)
00326     {
00327       logDebug("Constructing sampler for group '%s' as a union of %u samplers", jmg->getName().c_str(),
00328                (unsigned int)samplers.size());
00329       return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
00330     }
00331   }
00332 
00333   // if we've gotten here, just return joint sampler
00334   if (joint_sampler)
00335   {
00336     logDebug("Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
00337     return joint_sampler;
00338   }
00339 
00340   logDebug("No constraints sampler allocated for group '%s'", jmg->getName().c_str());
00341 
00342   return ConstraintSamplerPtr();
00343 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49