collision_robot.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/collision_detection/collision_robot.h>
00038 #include <limits>
00039 
00040 static inline bool validateScale(double scale)
00041 {
00042   if (scale < std::numeric_limits<double>::epsilon())
00043   {
00044     logError("Scale must be positive");
00045     return false;
00046   }
00047   if (scale > std::numeric_limits<double>::max())
00048   {
00049     logError("Scale must be finite");
00050     return false;
00051   }
00052   return true;
00053 }
00054 
00055 static inline bool validatePadding(double padding)
00056 {
00057   if (padding < 0.0)
00058   {
00059     logError("Padding cannot be negative");
00060     return false;
00061   }
00062   if (padding > std::numeric_limits<double>::max())
00063   {
00064     logError("Padding must be finite");
00065     return false;
00066   }
00067   return true;
00068 }
00069 
00070 collision_detection::CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, double padding,
00071                                                     double scale)
00072   : robot_model_(model)
00073 {
00074   if (!validateScale(scale))
00075     scale = 1.0;
00076   if (!validatePadding(padding))
00077     padding = 0.0;
00078 
00079   const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
00080   for (std::size_t i = 0; i < links.size(); ++i)
00081   {
00082     link_padding_[links[i]->getName()] = padding;
00083     link_scale_[links[i]->getName()] = scale;
00084   }
00085 }
00086 
00087 collision_detection::CollisionRobot::CollisionRobot(const CollisionRobot& other) : robot_model_(other.robot_model_)
00088 {
00089   link_padding_ = other.link_padding_;
00090   link_scale_ = other.link_scale_;
00091 }
00092 
00093 void collision_detection::CollisionRobot::setPadding(double padding)
00094 {
00095   if (!validatePadding(padding))
00096     return;
00097   std::vector<std::string> u;
00098   const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
00099   for (std::size_t i = 0; i < links.size(); ++i)
00100   {
00101     if (getLinkPadding(links[i]->getName()) != padding)
00102       u.push_back(links[i]->getName());
00103     link_padding_[links[i]->getName()] = padding;
00104   }
00105   if (!u.empty())
00106     updatedPaddingOrScaling(u);
00107 }
00108 
00109 void collision_detection::CollisionRobot::setScale(double scale)
00110 {
00111   if (!validateScale(scale))
00112     return;
00113   std::vector<std::string> u;
00114   const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
00115   for (std::size_t i = 0; i < links.size(); ++i)
00116   {
00117     if (getLinkScale(links[i]->getName()) != scale)
00118       u.push_back(links[i]->getName());
00119     link_scale_[links[i]->getName()] = scale;
00120   }
00121   if (!u.empty())
00122     updatedPaddingOrScaling(u);
00123 }
00124 
00125 void collision_detection::CollisionRobot::setLinkPadding(const std::string& link_name, double padding)
00126 {
00127   bool update = getLinkPadding(link_name) != padding;
00128   link_padding_[link_name] = padding;
00129   if (update)
00130   {
00131     std::vector<std::string> u(1, link_name);
00132     updatedPaddingOrScaling(u);
00133   }
00134 }
00135 
00136 double collision_detection::CollisionRobot::getLinkPadding(const std::string& link_name) const
00137 {
00138   std::map<std::string, double>::const_iterator it = link_padding_.find(link_name);
00139   if (it != link_padding_.end())
00140     return it->second;
00141   else
00142     return 0.0;
00143 }
00144 
00145 void collision_detection::CollisionRobot::setLinkPadding(const std::map<std::string, double>& padding)
00146 {
00147   std::vector<std::string> u;
00148   for (std::map<std::string, double>::const_iterator it = padding.begin(); it != padding.end(); ++it)
00149   {
00150     bool update = getLinkPadding(it->first) != it->second;
00151     link_padding_[it->first] = it->second;
00152     if (update)
00153       u.push_back(it->first);
00154   }
00155   if (!u.empty())
00156     updatedPaddingOrScaling(u);
00157 }
00158 
00159 const std::map<std::string, double>& collision_detection::CollisionRobot::getLinkPadding() const
00160 {
00161   return link_padding_;
00162 }
00163 
00164 void collision_detection::CollisionRobot::setLinkScale(const std::string& link_name, double scale)
00165 {
00166   bool update = getLinkScale(link_name) != scale;
00167   link_scale_[link_name] = scale;
00168   if (update)
00169   {
00170     std::vector<std::string> u(1, link_name);
00171     updatedPaddingOrScaling(u);
00172   }
00173 }
00174 
00175 double collision_detection::CollisionRobot::getLinkScale(const std::string& link_name) const
00176 {
00177   std::map<std::string, double>::const_iterator it = link_scale_.find(link_name);
00178   if (it != link_scale_.end())
00179     return it->second;
00180   else
00181     return 1.0;
00182 }
00183 
00184 void collision_detection::CollisionRobot::setLinkScale(const std::map<std::string, double>& scale)
00185 {
00186   std::vector<std::string> u;
00187   for (std::map<std::string, double>::const_iterator it = scale.begin(); it != scale.end(); ++it)
00188   {
00189     bool update = getLinkScale(it->first) != it->second;
00190     link_scale_[it->first] = it->second;
00191     if (update)
00192       u.push_back(it->first);
00193   }
00194   if (!u.empty())
00195     updatedPaddingOrScaling(u);
00196 }
00197 
00198 const std::map<std::string, double>& collision_detection::CollisionRobot::getLinkScale() const
00199 {
00200   return link_scale_;
00201 }
00202 
00203 void collision_detection::CollisionRobot::setPadding(const std::vector<moveit_msgs::LinkPadding>& padding)
00204 {
00205   std::vector<std::string> u;
00206   for (std::size_t i = 0; i < padding.size(); ++i)
00207   {
00208     bool update = getLinkPadding(padding[i].link_name) != padding[i].padding;
00209     link_padding_[padding[i].link_name] = padding[i].padding;
00210     if (update)
00211       u.push_back(padding[i].link_name);
00212   }
00213   if (!u.empty())
00214     updatedPaddingOrScaling(u);
00215 }
00216 
00217 void collision_detection::CollisionRobot::setScale(const std::vector<moveit_msgs::LinkScale>& scale)
00218 {
00219   std::vector<std::string> u;
00220   for (std::size_t i = 0; i < scale.size(); ++i)
00221   {
00222     bool update = getLinkScale(scale[i].link_name) != scale[i].scale;
00223     link_scale_[scale[i].link_name] = scale[i].scale;
00224     if (update)
00225       u.push_back(scale[i].link_name);
00226   }
00227   if (!u.empty())
00228     updatedPaddingOrScaling(u);
00229 }
00230 
00231 void collision_detection::CollisionRobot::getPadding(std::vector<moveit_msgs::LinkPadding>& padding) const
00232 {
00233   padding.clear();
00234   for (std::map<std::string, double>::const_iterator it = link_padding_.begin(); it != link_padding_.end(); ++it)
00235   {
00236     moveit_msgs::LinkPadding lp;
00237     lp.link_name = it->first;
00238     lp.padding = it->second;
00239     padding.push_back(lp);
00240   }
00241 }
00242 
00243 void collision_detection::CollisionRobot::getScale(std::vector<moveit_msgs::LinkScale>& scale) const
00244 {
00245   scale.clear();
00246   for (std::map<std::string, double>::const_iterator it = link_scale_.begin(); it != link_scale_.end(); ++it)
00247   {
00248     moveit_msgs::LinkScale ls;
00249     ls.link_name = it->first;
00250     ls.scale = it->second;
00251     scale.push_back(ls);
00252   }
00253 }
00254 
00255 void collision_detection::CollisionRobot::updatedPaddingOrScaling(const std::vector<std::string>& links)
00256 {
00257 }


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