moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 <rclcpp/logger.hpp>
39 #include <rclcpp/logging.hpp>
40 #include <limits>
41 #include <moveit/utils/logger.hpp>
42 
43 namespace
44 {
45 rclcpp::Logger getLogger()
46 {
47  return moveit::getLogger("moveit.core.collision_detection_env");
48 }
49 } // namespace
50 
51 static inline bool validateScale(const double scale)
52 {
53  if (scale < std::numeric_limits<double>::epsilon())
54  {
55  RCLCPP_ERROR(getLogger(), "Scale must be positive");
56  return false;
57  }
58  if (scale > std::numeric_limits<double>::max())
59  {
60  RCLCPP_ERROR(getLogger(), "Scale must be finite");
61  return false;
62  }
63  return true;
64 }
65 
66 static inline bool validatePadding(const double padding)
67 {
68  if (padding < 0.0)
69  {
70  RCLCPP_ERROR(getLogger(), "Padding cannot be negative");
71  return false;
72  }
73  if (padding > std::numeric_limits<double>::max())
74  {
75  RCLCPP_ERROR(getLogger(), "Padding must be finite");
76  return false;
77  }
78  return true;
79 }
80 
81 namespace collision_detection
82 {
83 CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, double padding, double scale)
84  : robot_model_(model), world_(std::make_shared<World>()), world_const_(world_)
85 {
86  if (!validateScale(scale))
87  scale = 1.0;
88  if (!validatePadding(padding))
89  padding = 0.0;
90 
91  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
92  for (const auto link : links)
93  {
94  link_padding_[link->getName()] = padding;
95  link_scale_[link->getName()] = scale;
96  }
97 }
98 
99 CollisionEnv::CollisionEnv(const moveit::core::RobotModelConstPtr& model, const WorldPtr& world, double padding,
100  double scale)
101  : robot_model_(model), world_(world), world_const_(world_)
102 {
103  if (!validateScale(scale))
104  scale = 1.0;
105  if (!validatePadding(padding))
106  padding = 0.0;
107 
108  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
109  for (const auto link : links)
110  {
111  link_padding_[link->getName()] = padding;
112  link_scale_[link->getName()] = scale;
113  }
114 }
115 
116 CollisionEnv::CollisionEnv(const CollisionEnv& other, const WorldPtr& world)
117  : robot_model_(other.robot_model_), world_(world), world_const_(world)
118 {
120  link_scale_ = other.link_scale_;
121 }
122 void CollisionEnv::setPadding(const double padding)
123 {
124  if (!validatePadding(padding))
125  return;
126  std::vector<std::string> u;
127  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
128  for (const auto link : links)
129  {
130  if (getLinkPadding(link->getName()) != padding)
131  u.push_back(link->getName());
132  link_padding_[link->getName()] = padding;
133  }
134  if (!u.empty())
136 }
137 
138 void CollisionEnv::setScale(const double scale)
139 {
140  if (!validateScale(scale))
141  return;
142  std::vector<std::string> u;
143  const std::vector<const moveit::core::LinkModel*>& links = robot_model_->getLinkModelsWithCollisionGeometry();
144  for (const auto link : links)
145  {
146  if (getLinkScale(link->getName()) != scale)
147  u.push_back(link->getName());
148  link_scale_[link->getName()] = scale;
149  }
150  if (!u.empty())
152 }
153 
154 void CollisionEnv::setLinkPadding(const std::string& link_name, const double padding)
155 {
156  validatePadding(padding);
157  const bool update = getLinkPadding(link_name) != padding;
158  link_padding_[link_name] = padding;
159  if (update)
160  {
161  std::vector<std::string> u(1, link_name);
163  }
164 }
165 
166 double CollisionEnv::getLinkPadding(const std::string& link_name) const
167 {
168  auto it = link_padding_.find(link_name);
169  if (it != link_padding_.end())
170  {
171  return it->second;
172  }
173  else
174  {
175  return 0.0;
176  }
177 }
178 
179 void CollisionEnv::setLinkPadding(const std::map<std::string, double>& padding)
180 {
181  std::vector<std::string> u;
182  for (const auto& link_pad_pair : padding)
183  {
184  validatePadding(link_pad_pair.second);
185  const bool update = getLinkPadding(link_pad_pair.first) != link_pad_pair.second;
186  link_padding_[link_pad_pair.first] = link_pad_pair.second;
187  if (update)
188  u.push_back(link_pad_pair.first);
189  }
190  if (!u.empty())
192 }
193 
194 const std::map<std::string, double>& CollisionEnv::getLinkPadding() const
195 {
196  return link_padding_;
197 }
198 
199 void CollisionEnv::setLinkScale(const std::string& link_name, const double scale)
200 {
201  validateScale(scale);
202  const bool update = getLinkScale(link_name) != scale;
203  link_scale_[link_name] = scale;
204  if (update)
205  {
206  std::vector<std::string> u(1, link_name);
208  }
209 }
210 
211 double CollisionEnv::getLinkScale(const std::string& link_name) const
212 {
213  const auto it = link_scale_.find(link_name);
214  if (it != link_scale_.end())
215  {
216  return it->second;
217  }
218  else
219  {
220  return 1.0;
221  }
222 }
223 
224 void CollisionEnv::setLinkScale(const std::map<std::string, double>& scale)
225 {
226  std::vector<std::string> u;
227  for (const auto& link_scale_pair : scale)
228  {
229  const bool update = getLinkScale(link_scale_pair.first) != link_scale_pair.second;
230  link_scale_[link_scale_pair.first] = link_scale_pair.second;
231  if (update)
232  u.push_back(link_scale_pair.first);
233  }
234  if (!u.empty())
236 }
237 
238 const std::map<std::string, double>& CollisionEnv::getLinkScale() const
239 {
240  return link_scale_;
241 }
242 
243 void CollisionEnv::setPadding(const std::vector<moveit_msgs::msg::LinkPadding>& padding)
244 {
245  std::vector<std::string> u;
246  for (const auto& p : padding)
247  {
248  validatePadding(p.padding);
249  const bool update = getLinkPadding(p.link_name) != p.padding;
250  link_padding_[p.link_name] = p.padding;
251  if (update)
252  u.push_back(p.link_name);
253  }
254  if (!u.empty())
256 }
257 
258 void CollisionEnv::setScale(const std::vector<moveit_msgs::msg::LinkScale>& scale)
259 {
260  std::vector<std::string> u;
261  for (const auto& s : scale)
262  {
263  validateScale(s.scale);
264  const bool update = getLinkScale(s.link_name) != s.scale;
265  link_scale_[s.link_name] = s.scale;
266  if (update)
267  u.push_back(s.link_name);
268  }
269  if (!u.empty())
271 }
272 
273 void CollisionEnv::getPadding(std::vector<moveit_msgs::msg::LinkPadding>& padding) const
274 {
275  padding.clear();
276  for (const auto& lp : link_padding_)
277  {
278  moveit_msgs::msg::LinkPadding lp_msg;
279  lp_msg.link_name = lp.first;
280  lp_msg.padding = lp.second;
281  padding.push_back(lp_msg);
282  }
283 }
284 
285 void CollisionEnv::getScale(std::vector<moveit_msgs::msg::LinkScale>& scale) const
286 {
287  scale.clear();
288  for (const auto& ls : link_scale_)
289  {
290  moveit_msgs::msg::LinkScale ls_msg;
291  ls_msg.link_name = ls.first;
292  ls_msg.scale = ls.second;
293  scale.push_back(ls_msg);
294  }
295 }
296 
297 void CollisionEnv::updatedPaddingOrScaling(const std::vector<std::string>& /*links*/)
298 {
299 }
300 
301 void CollisionEnv::setWorld(const WorldPtr& world)
302 {
303  world_ = world;
304  if (!world_)
305  world_ = std::make_shared<World>();
306 
307  world_const_ = world;
308 }
309 
311  const moveit::core::RobotState& state) const
312 {
313  checkSelfCollision(req, res, state);
314  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
315  checkRobotCollision(req, res, state);
316 }
317 
319  const moveit::core::RobotState& state, const AllowedCollisionMatrix& acm) const
320 {
321  checkSelfCollision(req, res, state, acm);
322  if (!res.collision || (req.contacts && res.contacts.size() < req.max_contacts))
323  checkRobotCollision(req, res, state, acm);
324 }
325 } // end of namespace collision_detection
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
Provides the interface to the individual collision checking libraries.
Definition: collision_env.h:52
virtual void setWorld(const WorldPtr &world)
void getPadding(std::vector< moveit_msgs::msg::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.
void setLinkPadding(const std::string &link_name, double padding)
Set the link padding for a particular link.
moveit::core::RobotModelConstPtr robot_model_
The kinematic model corresponding to this collision model.
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,...
const std::map< std::string, double > & getLinkScale() const
Get the link scaling as a map (from link names to scale value)
void setScale(double scale)
Set the link scaling (for every link)
void setPadding(double padding)
Set the link padding (for every link)
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....
void getScale(std::vector< moveit_msgs::msg::LinkScale > &scale) const
Get the link scaling as a vector of messages.
std::map< std::string, double > link_padding_
The internally maintained map (from link names to padding)
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...
const std::map< std::string, double > & getLinkPadding() const
Get the link paddings as a map (from link names to padding value)
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,...
std::map< std::string, double > link_scale_
The internally maintained map (from link names to scaling)
Maintain a representation of the environment.
Definition: world.h:59
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void update(moveit::core::RobotState *self, bool force, std::string &category)
Definition: robot_state.cpp:47
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition: logger.cpp:79
Representation of a collision checking request.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
std::size_t max_contacts
Overall maximum number of contacts to compute.
Representation of a collision checking result.
bool collision
True if collision was found, false otherwise.