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 &model, double padding, double scale)
00071 : robot_model_(model)
00072 {
00073 if (!validateScale(scale))
00074 scale = 1.0;
00075 if (!validatePadding(padding))
00076 padding = 0.0;
00077
00078 const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
00079 for (std::size_t i = 0 ; i < links.size() ; ++i)
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) : robot_model_(other.robot_model_)
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 = robot_model_->getLinkModelsWithCollisionGeometry();
00098 for (std::size_t i = 0 ; i < links.size() ; ++i)
00099 {
00100 if (getLinkPadding(links[i]->getName()) != padding)
00101 u.push_back(links[i]->getName());
00102 link_padding_[links[i]->getName()] = padding;
00103 }
00104 if (!u.empty())
00105 updatedPaddingOrScaling(u);
00106 }
00107
00108 void collision_detection::CollisionRobot::setScale(double scale)
00109 {
00110 if (!validateScale(scale))
00111 return;
00112 std::vector<std::string> u;
00113 const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
00114 for (std::size_t i = 0 ; i < links.size() ; ++i)
00115 {
00116 if (getLinkScale(links[i]->getName()) != scale)
00117 u.push_back(links[i]->getName());
00118 link_scale_[links[i]->getName()] = scale;
00119 }
00120 if (!u.empty())
00121 updatedPaddingOrScaling(u);
00122 }
00123
00124 void collision_detection::CollisionRobot::setLinkPadding(const std::string &link_name, double padding)
00125 {
00126 bool update = getLinkPadding(link_name) != padding;
00127 link_padding_[link_name] = padding;
00128 if (update)
00129 {
00130 std::vector<std::string> u(1, link_name);
00131 updatedPaddingOrScaling(u);
00132 }
00133 }
00134
00135 double collision_detection::CollisionRobot::getLinkPadding(const std::string &link_name) const
00136 {
00137 std::map<std::string, double>::const_iterator it = link_padding_.find(link_name);
00138 if (it != link_padding_.end())
00139 return it->second;
00140 else
00141 return 0.0;
00142 }
00143
00144 void collision_detection::CollisionRobot::setLinkPadding(const std::map<std::string, double> &padding)
00145 {
00146 std::vector<std::string> u;
00147 for (std::map<std::string, double>::const_iterator it = padding.begin() ; it != padding.end() ; ++it)
00148 {
00149 bool update = getLinkPadding(it->first) != it->second;
00150 link_padding_[it->first] = it->second;
00151 if (update)
00152 u.push_back(it->first);
00153 }
00154 if (!u.empty())
00155 updatedPaddingOrScaling(u);
00156 }
00157
00158 const std::map<std::string, double> &collision_detection::CollisionRobot::getLinkPadding() const
00159 {
00160 return link_padding_;
00161 }
00162
00163 void collision_detection::CollisionRobot::setLinkScale(const std::string &link_name, double scale)
00164 {
00165 bool update = getLinkScale(link_name) != scale;
00166 link_scale_[link_name] = scale;
00167 if (update)
00168 {
00169 std::vector<std::string> u(1, link_name);
00170 updatedPaddingOrScaling(u);
00171 }
00172 }
00173
00174 double collision_detection::CollisionRobot::getLinkScale(const std::string &link_name) const
00175 {
00176 std::map<std::string, double>::const_iterator it = link_scale_.find(link_name);
00177 if (it != link_scale_.end())
00178 return it->second;
00179 else
00180 return 1.0;
00181 }
00182
00183 void collision_detection::CollisionRobot::setLinkScale(const std::map<std::string, double> &scale)
00184 {
00185 std::vector<std::string> u;
00186 for (std::map<std::string, double>::const_iterator it = scale.begin() ; it != scale.end() ; ++it)
00187 {
00188 bool update = getLinkScale(it->first) != it->second;
00189 link_scale_[it->first] = it->second;
00190 if (update)
00191 u.push_back(it->first);
00192 }
00193 if (!u.empty())
00194 updatedPaddingOrScaling(u);
00195 }
00196
00197 const std::map<std::string, double> &collision_detection::CollisionRobot::getLinkScale() const
00198 {
00199 return link_scale_;
00200 }
00201
00202 void collision_detection::CollisionRobot::setPadding(const std::vector<moveit_msgs::LinkPadding> &padding)
00203 {
00204 std::vector<std::string> u;
00205 for (std::size_t i = 0 ; i < padding.size() ; ++i)
00206 {
00207 bool update = getLinkPadding(padding[i].link_name) != padding[i].padding;
00208 link_padding_[padding[i].link_name] = padding[i].padding;
00209 if (update)
00210 u.push_back(padding[i].link_name);
00211 }
00212 if (!u.empty())
00213 updatedPaddingOrScaling(u);
00214 }
00215
00216 void collision_detection::CollisionRobot::setScale(const std::vector<moveit_msgs::LinkScale> &scale)
00217 {
00218 std::vector<std::string> u;
00219 for (std::size_t i = 0 ; i < scale.size() ; ++i)
00220 {
00221 bool update = getLinkScale(scale[i].link_name) != scale[i].scale;
00222 link_scale_[scale[i].link_name] = scale[i].scale;
00223 if (update)
00224 u.push_back(scale[i].link_name);
00225 }
00226 if (!u.empty())
00227 updatedPaddingOrScaling(u);
00228 }
00229
00230 void collision_detection::CollisionRobot::getPadding(std::vector<moveit_msgs::LinkPadding> &padding) const
00231 {
00232 padding.clear();
00233 for (std::map<std::string, double>::const_iterator it = link_padding_.begin() ; it != link_padding_.end() ; ++it)
00234 {
00235 moveit_msgs::LinkPadding lp;
00236 lp.link_name = it->first;
00237 lp.padding = it->second;
00238 padding.push_back(lp);
00239 }
00240 }
00241
00242 void collision_detection::CollisionRobot::getScale(std::vector<moveit_msgs::LinkScale> &scale) const
00243 {
00244 scale.clear();
00245 for (std::map<std::string, double>::const_iterator it = link_scale_.begin() ; it != link_scale_.end() ; ++it)
00246 {
00247 moveit_msgs::LinkScale ls;
00248 ls.link_name = it->first;
00249 ls.scale = it->second;
00250 scale.push_back(ls);
00251 }
00252 }
00253
00254 void collision_detection::CollisionRobot::updatedPaddingOrScaling(const std::vector<std::string> &links)
00255 {
00256 }