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  CONSOLE_BRIDGE_logError("Scale must be positive");
45  return false;
46  }
47  if (scale > std::numeric_limits<double>::max())
48  {
49  CONSOLE_BRIDGE_logError("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  CONSOLE_BRIDGE_logError("Padding cannot be negative");
60  return false;
61  }
62  if (padding > std::numeric_limits<double>::max())
63  {
64  CONSOLE_BRIDGE_logError("Padding must be finite");
65  return false;
66  }
67  return true;
68 }
69 
70 collision_detection::CollisionRobot::CollisionRobot(const robot_model::RobotModelConstPtr& model, // NOLINT
71  double padding, double scale)
72  : robot_model_(model)
73 {
74  if (!validateScale(scale))
75  scale = 1.0;
76  if (!validatePadding(padding))
77  padding = 0.0;
78 
79  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
80  for (auto link : links)
81  {
82  link_padding_[link->getName()] = padding;
83  link_scale_[link->getName()] = scale;
84  }
85 }
86 
88 {
90  link_scale_ = other.link_scale_;
91 }
92 
94 {
95  if (!validatePadding(padding))
96  return;
97  std::vector<std::string> u;
98  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
99  for (auto link : links)
100  {
101  if (getLinkPadding(link->getName()) != padding)
102  u.push_back(link->getName());
103  link_padding_[link->getName()] = padding;
104  }
105  if (!u.empty())
107 }
108 
110 {
111  if (!validateScale(scale))
112  return;
113  std::vector<std::string> u;
114  const std::vector<const robot_model::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
115  for (auto link : links)
116  {
117  if (getLinkScale(link->getName()) != scale)
118  u.push_back(link->getName());
119  link_scale_[link->getName()] = scale;
120  }
121  if (!u.empty())
123 }
124 
125 void collision_detection::CollisionRobot::setLinkPadding(const std::string& link_name, double padding)
126 {
127  bool update = getLinkPadding(link_name) != padding;
128  link_padding_[link_name] = padding;
129  if (update)
130  {
131  std::vector<std::string> u(1, link_name);
133  }
134 }
135 
136 double collision_detection::CollisionRobot::getLinkPadding(const std::string& link_name) const
137 {
138  auto it = link_padding_.find(link_name);
139  if (it != link_padding_.end())
140  return it->second;
141  else
142  return 0.0;
143 }
144 
145 void collision_detection::CollisionRobot::setLinkPadding(const std::map<std::string, double>& padding)
146 {
147  std::vector<std::string> u;
148  for (const auto& link_pad_pair : padding)
149  {
150  bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
151  link_padding_[link_pad_pair.first] = link_pad_pair.second;
152  if (update)
153  u.push_back(link_pad_pair.first);
154  }
155  if (!u.empty())
157 }
158 
159 const std::map<std::string, double>& collision_detection::CollisionRobot::getLinkPadding() const
160 {
161  return link_padding_;
162 }
163 
164 void collision_detection::CollisionRobot::setLinkScale(const std::string& link_name, double scale)
165 {
166  bool update = getLinkScale(link_name) != scale;
167  link_scale_[link_name] = scale;
168  if (update)
169  {
170  std::vector<std::string> u(1, link_name);
172  }
173 }
174 
175 double collision_detection::CollisionRobot::getLinkScale(const std::string& link_name) const
176 {
177  auto it = link_scale_.find(link_name);
178  if (it != link_scale_.end())
179  return it->second;
180  else
181  return 1.0;
182 }
183 
184 void collision_detection::CollisionRobot::setLinkScale(const std::map<std::string, double>& scale)
185 {
186  std::vector<std::string> u;
187  for (const auto& link_scale_pair : scale)
188  {
189  bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
190  link_scale_[link_scale_pair.first] = link_scale_pair.second;
191  if (update)
192  u.push_back(link_scale_pair.first);
193  }
194  if (!u.empty())
196 }
197 
198 const std::map<std::string, double>& collision_detection::CollisionRobot::getLinkScale() const
199 {
200  return link_scale_;
201 }
202 
203 void collision_detection::CollisionRobot::setPadding(const std::vector<moveit_msgs::LinkPadding>& padding)
204 {
205  std::vector<std::string> u;
206  for (const auto& p : padding)
207  {
208  bool update = getLinkPadding(p.link_name) != p.padding;
209  link_padding_[p.link_name] = p.padding;
210  if (update)
211  u.push_back(p.link_name);
212  }
213  if (!u.empty())
215 }
216 
217 void collision_detection::CollisionRobot::setScale(const std::vector<moveit_msgs::LinkScale>& scale)
218 {
219  std::vector<std::string> u;
220  for (const auto& s : scale)
221  {
222  bool update = getLinkScale(s.link_name) != s.scale;
223  link_scale_[s.link_name] = s.scale;
224  if (update)
225  u.push_back(s.link_name);
226  }
227  if (!u.empty())
229 }
230 
231 void collision_detection::CollisionRobot::getPadding(std::vector<moveit_msgs::LinkPadding>& padding) const
232 {
233  padding.clear();
234  for (const auto& lp : link_padding_)
235  {
236  moveit_msgs::LinkPadding lp_msg;
237  lp_msg.link_name = lp.first;
238  lp_msg.padding = lp.second;
239  padding.push_back(lp_msg);
240  }
241 }
242 
243 void collision_detection::CollisionRobot::getScale(std::vector<moveit_msgs::LinkScale>& scale) const
244 {
245  scale.clear();
246  for (const auto& ls : link_scale_)
247  {
248  moveit_msgs::LinkScale ls_msg;
249  ls_msg.link_name = ls.first;
250  ls_msg.scale = ls.second;
251  scale.push_back(ls_msg);
252  }
253 }
254 
255 void collision_detection::CollisionRobot::updatedPaddingOrScaling(const std::vector<std::string>& links)
256 {
257 }
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)
#define CONSOLE_BRIDGE_logError(fmt,...)
XmlRpcServer s
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...
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)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44