collision_robot.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 #include <limits>
39 
40 static inline bool validateScale(double scale)
41 {
42  if (scale < std::numeric_limits<double>::epsilon())
43  {
44  ROS_ERROR_NAMED("collision_detection", "Scale must be positive");
45  return false;
46  }
47  if (scale > std::numeric_limits<double>::max())
48  {
49  ROS_ERROR_NAMED("collision_detection", "Scale must be finite");
50  return false;
51  }
52  return true;
53 }
54 
55 static inline bool validatePadding(double padding)
56 {
57  if (padding < 0.0)
58  {
59  ROS_ERROR_NAMED("collision_detection", "Padding cannot be negative");
60  return false;
61  }
62  if (padding > std::numeric_limits<double>::max())
63  {
64  ROS_ERROR_NAMED("collision_detection", "Padding must be finite");
65  return false;
66  }
67  return true;
68 }
69 
70 namespace collision_detection
71 {
72 CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, double padding, double scale)
73  : robot_model_(model)
74 {
75  if (!validateScale(scale))
76  scale = 1.0;
77  if (!validatePadding(padding))
78  padding = 0.0;
79 
80  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
81  for (auto link : links)
82  {
83  link_padding_[link->getName()] = padding;
84  link_scale_[link->getName()] = scale;
85  }
86 }
87 
89 {
91  link_scale_ = other.link_scale_;
92 }
93 
94 void CollisionRobot::setPadding(double padding)
95 {
96  if (!validatePadding(padding))
97  return;
98  std::vector<std::string> u;
99  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
100  for (auto link : links)
101  {
102  if (getLinkPadding(link->getName()) != padding)
103  u.push_back(link->getName());
104  link_padding_[link->getName()] = padding;
105  }
106  if (!u.empty())
108 }
109 
110 void CollisionRobot::setScale(double scale)
111 {
112  if (!validateScale(scale))
113  return;
114  std::vector<std::string> u;
115  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
116  for (auto link : links)
117  {
118  if (getLinkScale(link->getName()) != scale)
119  u.push_back(link->getName());
120  link_scale_[link->getName()] = scale;
121  }
122  if (!u.empty())
124 }
125 
126 void CollisionRobot::setLinkPadding(const std::string& link_name, double padding)
127 {
128  bool update = getLinkPadding(link_name) != padding;
129  link_padding_[link_name] = padding;
130  if (update)
131  {
132  std::vector<std::string> u(1, link_name);
134  }
135 }
136 
137 double CollisionRobot::getLinkPadding(const std::string& link_name) const
138 {
139  auto it = link_padding_.find(link_name);
140  if (it != link_padding_.end())
141  return it->second;
142  else
143  return 0.0;
144 }
145 
146 void CollisionRobot::setLinkPadding(const std::map<std::string, double>& padding)
147 {
148  std::vector<std::string> u;
149  for (const auto& link_pad_pair : padding)
150  {
151  bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
152  link_padding_[link_pad_pair.first] = link_pad_pair.second;
153  if (update)
154  u.push_back(link_pad_pair.first);
155  }
156  if (!u.empty())
158 }
159 
160 const std::map<std::string, double>& CollisionRobot::getLinkPadding() const
161 {
162  return link_padding_;
163 }
164 
165 void CollisionRobot::setLinkScale(const std::string& link_name, double scale)
166 {
167  bool update = getLinkScale(link_name) != scale;
168  link_scale_[link_name] = scale;
169  if (update)
170  {
171  std::vector<std::string> u(1, link_name);
173  }
174 }
175 
176 double CollisionRobot::getLinkScale(const std::string& link_name) const
177 {
178  auto it = link_scale_.find(link_name);
179  if (it != link_scale_.end())
180  return it->second;
181  else
182  return 1.0;
183 }
184 
185 void CollisionRobot::setLinkScale(const std::map<std::string, double>& scale)
186 {
187  std::vector<std::string> u;
188  for (const auto& link_scale_pair : scale)
189  {
190  bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
191  link_scale_[link_scale_pair.first] = link_scale_pair.second;
192  if (update)
193  u.push_back(link_scale_pair.first);
194  }
195  if (!u.empty())
197 }
198 
199 const std::map<std::string, double>& CollisionRobot::getLinkScale() const
200 {
201  return link_scale_;
202 }
203 
204 void CollisionRobot::setPadding(const std::vector<moveit_msgs::LinkPadding>& padding)
205 {
206  std::vector<std::string> u;
207  for (const auto& p : padding)
208  {
209  bool update = getLinkPadding(p.link_name) != p.padding;
210  link_padding_[p.link_name] = p.padding;
211  if (update)
212  u.push_back(p.link_name);
213  }
214  if (!u.empty())
216 }
217 
218 void CollisionRobot::setScale(const std::vector<moveit_msgs::LinkScale>& scale)
219 {
220  std::vector<std::string> u;
221  for (const auto& s : scale)
222  {
223  bool update = getLinkScale(s.link_name) != s.scale;
224  link_scale_[s.link_name] = s.scale;
225  if (update)
226  u.push_back(s.link_name);
227  }
228  if (!u.empty())
230 }
231 
232 void CollisionRobot::getPadding(std::vector<moveit_msgs::LinkPadding>& padding) const
233 {
234  padding.clear();
235  for (const auto& lp : link_padding_)
236  {
237  moveit_msgs::LinkPadding lp_msg;
238  lp_msg.link_name = lp.first;
239  lp_msg.padding = lp.second;
240  padding.push_back(lp_msg);
241  }
242 }
243 
244 void CollisionRobot::getScale(std::vector<moveit_msgs::LinkScale>& scale) const
245 {
246  scale.clear();
247  for (const auto& ls : link_scale_)
248  {
249  moveit_msgs::LinkScale ls_msg;
250  ls_msg.link_name = ls.first;
251  ls_msg.scale = ls.second;
252  scale.push_back(ls_msg);
253  }
254 }
255 
256 void CollisionRobot::updatedPaddingOrScaling(const std::vector<std::string>& links)
257 {
258 }
259 
260 } // end of namespace collision_detection
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)
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
XmlRpcServer s
void setPadding(double padding)
Set the link padding (for every link)
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 getPadding(std::vector< moveit_msgs::LinkPadding > &padding) const
Get the link padding 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 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 getScale(std::vector< moveit_msgs::LinkScale > &scale) const
Get the link scaling as a vector of messages.
void setLinkPadding(const std::string &link_name, double padding)
Set the link padding for a particular link.
#define ROS_ERROR_NAMED(name,...)
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Feb 17 2020 03:50:49