Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 &kmodel, double padding, double scale) : kmodel_(kmodel)
00071 {
00072 if (!validateScale(scale))
00073 scale = 1.0;
00074 if (!validatePadding(padding))
00075 padding = 0.0;
00076
00077 const std::vector<const robot_model::LinkModel*>& links = kmodel_->getLinkModels();
00078 for (std::size_t i = 0 ; i < links.size() ; ++i)
00079 if (links[i] && links[i]->getShape())
00080 {
00081 link_padding_[links[i]->getName()] = padding;
00082 link_scale_[links[i]->getName()] = scale;
00083 }
00084 }
00085
00086 collision_detection::CollisionRobot::CollisionRobot(const CollisionRobot &other) : kmodel_(other.kmodel_)
00087 {
00088 link_padding_ = other.link_padding_;
00089 link_scale_ = other.link_scale_;
00090 }
00091
00092 void collision_detection::CollisionRobot::setPadding(double padding)
00093 {
00094 if (!validatePadding(padding))
00095 return;
00096 std::vector<std::string> u;
00097 const std::vector<const robot_model::LinkModel*>& links = kmodel_->getLinkModels();
00098 for (std::size_t i = 0 ; i < links.size() ; ++i)
00099 if (links[i] && links[i]->getShape())
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 = kmodel_->getLinkModels();
00115 for (std::size_t i = 0 ; i < links.size() ; ++i)
00116 if (links[i] && links[i]->getShape())
00117 {
00118 if (getLinkScale(links[i]->getName()) != scale)
00119 u.push_back(links[i]->getName());
00120 link_scale_[links[i]->getName()] = scale;
00121 }
00122 if (!u.empty())
00123 updatedPaddingOrScaling(u);
00124 }
00125
00126 void collision_detection::CollisionRobot::setLinkPadding(const std::string &link_name, double padding)
00127 {
00128 bool update = getLinkPadding(link_name) != padding;
00129 link_padding_[link_name] = padding;
00130 if (update)
00131 {
00132 std::vector<std::string> u(1, link_name);
00133 updatedPaddingOrScaling(u);
00134 }
00135 }
00136
00137 double collision_detection::CollisionRobot::getLinkPadding(const std::string &link_name) const
00138 {
00139 std::map<std::string, double>::const_iterator it = link_padding_.find(link_name);
00140 if (it != link_padding_.end())
00141 return it->second;
00142 else
00143 return 0.0;
00144 }
00145
00146 void collision_detection::CollisionRobot::setLinkPadding(const std::map<std::string, double> &padding)
00147 {
00148 std::vector<std::string> u;
00149 for (std::map<std::string, double>::const_iterator it = padding.begin() ; it != padding.end() ; ++it)
00150 {
00151 bool update = getLinkPadding(it->first) != it->second;
00152 link_padding_[it->first] = it->second;
00153 if (update)
00154 u.push_back(it->first);
00155 }
00156 if (!u.empty())
00157 updatedPaddingOrScaling(u);
00158 }
00159
00160 const std::map<std::string, double> &collision_detection::CollisionRobot::getLinkPadding() const
00161 {
00162 return link_padding_;
00163 }
00164
00165 void collision_detection::CollisionRobot::setLinkScale(const std::string &link_name, double scale)
00166 {
00167 bool update = getLinkScale(link_name) != scale;
00168 link_scale_[link_name] = scale;
00169 if (update)
00170 {
00171 std::vector<std::string> u(1, link_name);
00172 updatedPaddingOrScaling(u);
00173 }
00174 }
00175
00176 double collision_detection::CollisionRobot::getLinkScale(const std::string &link_name) const
00177 {
00178 std::map<std::string, double>::const_iterator it = link_scale_.find(link_name);
00179 if (it != link_scale_.end())
00180 return it->second;
00181 else
00182 return 1.0;
00183 }
00184
00185 void collision_detection::CollisionRobot::setLinkScale(const std::map<std::string, double> &scale)
00186 {
00187 std::vector<std::string> u;
00188 for (std::map<std::string, double>::const_iterator it = scale.begin() ; it != scale.end() ; ++it)
00189 {
00190 bool update = getLinkScale(it->first) != it->second;
00191 link_scale_[it->first] = it->second;
00192 if (update)
00193 u.push_back(it->first);
00194 }
00195 if (!u.empty())
00196 updatedPaddingOrScaling(u);
00197 }
00198
00199 const std::map<std::string, double> &collision_detection::CollisionRobot::getLinkScale() const
00200 {
00201 return link_scale_;
00202 }
00203
00204 void collision_detection::CollisionRobot::setPadding(const std::vector<moveit_msgs::LinkPadding> &padding)
00205 {
00206 std::vector<std::string> u;
00207 for (std::size_t i = 0 ; i < padding.size() ; ++i)
00208 {
00209 bool update = getLinkPadding(padding[i].link_name) != padding[i].padding;
00210 link_padding_[padding[i].link_name] = padding[i].padding;
00211 if (update)
00212 u.push_back(padding[i].link_name);
00213 }
00214 if (!u.empty())
00215 updatedPaddingOrScaling(u);
00216 }
00217
00218 void collision_detection::CollisionRobot::setScale(const std::vector<moveit_msgs::LinkScale> &scale)
00219 {
00220 std::vector<std::string> u;
00221 for (std::size_t i = 0 ; i < scale.size() ; ++i)
00222 {
00223 bool update = getLinkScale(scale[i].link_name) != scale[i].scale;
00224 link_scale_[scale[i].link_name] = scale[i].scale;
00225 if (update)
00226 u.push_back(scale[i].link_name);
00227 }
00228 if (!u.empty())
00229 updatedPaddingOrScaling(u);
00230 }
00231
00232 void collision_detection::CollisionRobot::getPadding(std::vector<moveit_msgs::LinkPadding> &padding) const
00233 {
00234 padding.clear();
00235 for (std::map<std::string, double>::const_iterator it = link_padding_.begin() ; it != link_padding_.end() ; ++it)
00236 {
00237 moveit_msgs::LinkPadding lp;
00238 lp.link_name = it->first;
00239 lp.padding = it->second;
00240 padding.push_back(lp);
00241 }
00242 }
00243
00244 void collision_detection::CollisionRobot::getScale(std::vector<moveit_msgs::LinkScale> &scale) const
00245 {
00246 scale.clear();
00247 for (std::map<std::string, double>::const_iterator it = link_scale_.begin() ; it != link_scale_.end() ; ++it)
00248 {
00249 moveit_msgs::LinkScale ls;
00250 ls.link_name = it->first;
00251 ls.scale = it->second;
00252 scale.push_back(ls);
00253 }
00254 }
00255
00256 void collision_detection::CollisionRobot::updatedPaddingOrScaling(const std::vector<std::string> &links)
00257 {
00258 }