collision_env.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 the copyright holder 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, Jens Petit */
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 CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding, double scale)
73  : robot_model_(model), world_(new World()), world_const_(world_)
74 {
75  if (!validateScale(scale))
76  scale = 1.0;
77  if (!validatePadding(padding))
78  padding = 0.0;
79 
80  const std::vector<const moveit::core::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 
88 CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding,
89  double scale)
90  : robot_model_(model), world_(world), world_const_(world_)
91 {
92  if (!validateScale(scale))
93  scale = 1.0;
94  if (!validatePadding(padding))
95  padding = 0.0;
96 
97  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
98  for (auto link : links)
99  {
100  link_padding_[link->getName()] = padding;
101  link_scale_[link->getName()] = scale;
102  }
103 }
104 
105 CollisionEnv::CollisionEnv(const CollisionEnv& other, const WorldPtr& world)
106  : robot_model_(other.robot_model_), world_(world), world_const_(world)
107 {
109  link_scale_ = other.link_scale_;
110 }
111 void CollisionEnv::setPadding(double padding)
112 {
113  if (!validatePadding(padding))
114  return;
115  std::vector<std::string> u;
116  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
117  for (auto link : links)
118  {
119  if (getLinkPadding(link->getName()) != padding)
120  u.push_back(link->getName());
121  link_padding_[link->getName()] = padding;
122  }
123  if (!u.empty())
125 }
126 
127 void CollisionEnv::setScale(double scale)
128 {
129  if (!validateScale(scale))
130  return;
131  std::vector<std::string> u;
132  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
133  for (auto link : links)
134  {
135  if (getLinkScale(link->getName()) != scale)
136  u.push_back(link->getName());
137  link_scale_[link->getName()] = scale;
138  }
139  if (!u.empty())
141 }
142 
143 void CollisionEnv::setLinkPadding(const std::string& link_name, double padding)
144 {
145  validatePadding(padding);
146  bool update = getLinkPadding(link_name) != padding;
147  link_padding_[link_name] = padding;
148  if (update)
149  {
150  std::vector<std::string> u(1, link_name);
152  }
153 }
154 
155 double CollisionEnv::getLinkPadding(const std::string& link_name) const
156 {
157  auto it = link_padding_.find(link_name);
158  if (it != link_padding_.end())
159  return it->second;
160  else
161  return 0.0;
162 }
163 
164 void CollisionEnv::setLinkPadding(const std::map<std::string, double>& padding)
165 {
166  std::vector<std::string> u;
167  for (const auto& link_pad_pair : padding)
168  {
169  validatePadding(link_pad_pair.second);
170  bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
171  link_padding_[link_pad_pair.first] = link_pad_pair.second;
172  if (update)
173  u.push_back(link_pad_pair.first);
174  }
175  if (!u.empty())
177 }
178 
179 const std::map<std::string, double>& CollisionEnv::getLinkPadding() const
180 {
181  return link_padding_;
182 }
183 
184 void CollisionEnv::setLinkScale(const std::string& link_name, double scale)
185 {
186  validateScale(scale);
187  bool update = getLinkScale(link_name) != scale;
188  link_scale_[link_name] = scale;
189  if (update)
190  {
191  std::vector<std::string> u(1, link_name);
193  }
194 }
195 
196 double CollisionEnv::getLinkScale(const std::string& link_name) const
197 {
198  auto it = link_scale_.find(link_name);
199  if (it != link_scale_.end())
200  return it->second;
201  else
202  return 1.0;
203 }
204 
205 void CollisionEnv::setLinkScale(const std::map<std::string, double>& scale)
206 {
207  std::vector<std::string> u;
208  for (const auto& link_scale_pair : scale)
209  {
210  bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
211  link_scale_[link_scale_pair.first] = link_scale_pair.second;
212  if (update)
213  u.push_back(link_scale_pair.first);
214  }
215  if (!u.empty())
217 }
218 
219 const std::map<std::string, double>& CollisionEnv::getLinkScale() const
220 {
221  return link_scale_;
222 }
223 
224 void CollisionEnv::setPadding(const std::vector<moveit_msgs::LinkPadding>& padding)
225 {
226  std::vector<std::string> u;
227  for (const auto& p : padding)
228  {
229  validatePadding(p.padding);
230  bool update = getLinkPadding(p.link_name) != p.padding;
231  link_padding_[p.link_name] = p.padding;
232  if (update)
233  u.push_back(p.link_name);
234  }
235  if (!u.empty())
237 }
238 
239 void CollisionEnv::setScale(const std::vector<moveit_msgs::LinkScale>& scale)
240 {
241  std::vector<std::string> u;
242  for (const auto& s : scale)
243  {
244  validateScale(s.scale);
245  bool update = getLinkScale(s.link_name) != s.scale;
246  link_scale_[s.link_name] = s.scale;
247  if (update)
248  u.push_back(s.link_name);
249  }
250  if (!u.empty())
252 }
253 
254 void CollisionEnv::getPadding(std::vector<moveit_msgs::LinkPadding>& padding) const
255 {
256  padding.clear();
257  for (const auto& lp : link_padding_)
258  {
259  moveit_msgs::LinkPadding lp_msg;
260  lp_msg.link_name = lp.first;
261  lp_msg.padding = lp.second;
262  padding.push_back(lp_msg);
263  }
264 }
265 
266 void CollisionEnv::getScale(std::vector<moveit_msgs::LinkScale>& scale) const
267 {
268  scale.clear();
269  for (const auto& ls : link_scale_)
270  {
271  moveit_msgs::LinkScale ls_msg;
272  ls_msg.link_name = ls.first;
273  ls_msg.scale = ls.second;
274  scale.push_back(ls_msg);
275  }
276 }
277 
278 void CollisionEnv::updatedPaddingOrScaling(const std::vector<std::string>& /*links*/)
279 {
280 }
281 
282 void CollisionEnv::setWorld(const WorldPtr& world)
283 {
284  world_ = world;
285  if (!world_)
286  world_ = std::make_shared<World>();
287 
288  world_const_ = world;
289 }
290 
292  const moveit::core::RobotState& state) const
293 {
294  checkSelfCollision(req, res, state);
295  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
296  checkRobotCollision(req, res, state);
297 }
298 
300  const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const
301 {
302  checkSelfCollision(req, res, state, acm);
303  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
304  checkRobotCollision(req, res, state, acm);
305 }
306 
307 } // end of namespace collision_detection
collision_detection::CollisionEnv::getScale
void getScale(std::vector< moveit_msgs::LinkScale > &scale) const
Get the link scaling as a vector of messages.
Definition: collision_env.cpp:266
collision_detection::CollisionEnv::world_
WorldPtr world_
Definition: collision_env.h:356
s
XmlRpcServer s
collision_detection::CollisionEnv::link_padding_
std::map< std::string, double > link_padding_
The internally maintained map (from link names to padding)
Definition: collision_env.h:350
collision_detection::CollisionEnv::checkRobotCollision
virtual void checkRobotCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const =0
Check whether the robot model is in collision with the world. Any collisions between a robot link and...
collision_detection::World
Maintain a representation of the environment.
Definition: world.h:59
collision_detection::CollisionEnv::checkSelfCollision
virtual void checkSelfCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const =0
Check for robot self collision. Any collision between any pair of links is checked for,...
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
collision_detection::CollisionEnv::checkCollision
virtual void checkCollision(const CollisionRequest &req, CollisionResult &res, const moveit::core::RobotState &state) const
Check whether the robot model is in collision with itself or the world at a particular state....
Definition: collision_env.cpp:291
collision_detection::CollisionEnv::setLinkPadding
void setLinkPadding(const std::string &link_name, double padding)
Set the link padding for a particular link.
Definition: collision_env.cpp:143
collision_detection::CollisionRequest
Representation of a collision checking request.
Definition: include/moveit/collision_detection/collision_common.h:216
collision_detection::CollisionEnv::setLinkScale
void setLinkScale(const std::string &link_name, double scale)
Set the scaling for a particular link.
Definition: collision_env.cpp:184
collision_detection::AllowedCollisionMatrix
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Definition: collision_matrix.h:112
collision_detection::CollisionEnv::setPadding
void setPadding(double padding)
Set the link padding (for every link)
Definition: collision_env.cpp:111
update
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
collision_detection::CollisionResult
Representation of a collision checking result.
Definition: include/moveit/collision_detection/collision_common.h:177
collision_env.h
collision_detection::CollisionEnv::world_const_
WorldConstPtr world_const_
Definition: collision_env.h:357
collision_detection::CollisionRequest::max_contacts
std::size_t max_contacts
Overall maximum number of contacts to compute.
Definition: include/moveit/collision_detection/collision_common.h:245
collision_detection::CollisionEnv::setScale
void setScale(double scale)
Set the link scaling (for every link)
Definition: collision_env.cpp:127
collision_detection::CollisionResult::contacts
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
Definition: include/moveit/collision_detection/collision_common.h:209
collision_detection::CollisionEnv::getLinkScale
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
Definition: collision_env.cpp:219
collision_detection::CollisionEnv::getPadding
void getPadding(std::vector< moveit_msgs::LinkPadding > &padding) const
Get the link padding as a vector of messages.
Definition: collision_env.cpp:254
collision_detection::CollisionRequest::contacts
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
Definition: include/moveit/collision_detection/collision_common.h:242
collision_detection::CollisionEnv::getLinkPadding
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
Definition: collision_env.cpp:179
collision_detection::CollisionResult::collision
bool collision
True if collision was found, false otherwise.
Definition: include/moveit/collision_detection/collision_common.h:200
validateScale
static bool validateScale(double scale)
Definition: collision_env.cpp:40
collision_detection::CollisionEnv::setWorld
virtual void setWorld(const WorldPtr &world)
Definition: collision_env.cpp:282
validatePadding
static bool validatePadding(double padding)
Definition: collision_env.cpp:55
collision_detection::CollisionEnv::CollisionEnv
CollisionEnv()=delete
collision_detection::CollisionEnv::robot_model_
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
Definition: collision_env.h:347
collision_detection::CollisionEnv::link_scale_
std::map< std::string, double > link_scale_
The internally maintained map (from link names to scaling)
Definition: collision_env.h:353
collision_detection::CollisionEnv
Provides the interface to the individual collision checking libraries.
Definition: collision_env.h:83
collision_detection::CollisionEnv::updatedPaddingOrScaling
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,...
Definition: collision_env.cpp:278
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Dec 3 2021 03:23:21