42 if (scale < std::numeric_limits<double>::epsilon())
47 if (scale > std::numeric_limits<double>::max())
62 if (padding > std::numeric_limits<double>::max())
73 double padding,
double scale)
81 const std::vector<const robot_model::LinkModel*>& links =
robot_model_->getLinkModelsWithCollisionGeometry();
82 for (
auto link : links)
99 std::vector<std::string> u;
100 const std::vector<const robot_model::LinkModel*>& links =
robot_model_->getLinkModelsWithCollisionGeometry();
101 for (
auto link : links)
104 u.push_back(link->getName());
115 std::vector<std::string> u;
116 const std::vector<const robot_model::LinkModel*>& links =
robot_model_->getLinkModelsWithCollisionGeometry();
117 for (
auto link : links)
120 u.push_back(link->getName());
133 std::vector<std::string> u(1, link_name);
149 std::vector<std::string> u;
150 for (
const auto& link_pad_pair : padding)
155 u.push_back(link_pad_pair.first);
172 std::vector<std::string> u(1, link_name);
188 std::vector<std::string> u;
189 for (
const auto& link_scale_pair : scale)
192 link_scale_[link_scale_pair.first] = link_scale_pair.second;
194 u.push_back(link_scale_pair.first);
207 std::vector<std::string> u;
208 for (
const auto& p : padding)
213 u.push_back(p.link_name);
221 std::vector<std::string> u;
222 for (
const auto&
s : scale)
227 u.push_back(
s.link_name);
238 moveit_msgs::LinkPadding lp_msg;
239 lp_msg.link_name = lp.first;
240 lp_msg.padding = lp.second;
241 padding.push_back(lp_msg);
250 moveit_msgs::LinkScale ls_msg;
251 ls_msg.link_name = ls.first;
252 ls_msg.scale = ls.second;
253 scale.push_back(ls_msg);
void setScale(double scale)
Set the link scaling (for every link)
static bool validatePadding(double padding)
static bool validateScale(double scale)
std::map< std::string, double > link_scale_
The internally maintained map (from link names to scaling)
void setPadding(double padding)
Set the link padding (for every link)
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
CollisionRobot(const robot_model::RobotModelConstPtr &model, double padding=0.0, double scale=1.0)
Constructor.
virtual void updatedPaddingOrScaling(const std::vector< std::string > &links)
When the scale or padding is changed for a set of links by any of the functions in this class...
Generic interface to collision detection.
void getScale(std::vector< moveit_msgs::LinkScale > &scale) const
Get the link scaling as a vector of messages.
robot_model::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void getPadding(std::vector< moveit_msgs::LinkPadding > &padding) const
Get the link padding as a vector of messages.
void setLinkScale(const std::string &link_name, double scale)
Set the scaling for a particular link.
This class represents a collision model of the robot and can be used for self collision checks (to ch...
std::map< std::string, double > link_padding_
The internally maintained map (from link names to padding)
void setLinkPadding(const std::string &link_name, double padding)
Set the link padding for a particular link.
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
#define ROS_ERROR_NAMED(name,...)