collision_matrix.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, E. Gil Jones */
36 
38 #include <functional>
39 #include <iomanip>
40 
41 namespace collision_detection
42 {
44 {
45 }
46 
47 AllowedCollisionMatrix::AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed)
48 {
49  for (std::size_t i = 0; i < names.size(); ++i)
50  for (std::size_t j = i; j < names.size(); ++j)
51  setEntry(names[i], names[j], allowed);
52 }
53 
55 {
56  // load collision defaults
57  for (const std::string& name : srdf.getNoDefaultCollisionLinks())
59  // re-enable specific collision pairs
60  for (auto const& collision : srdf.getEnabledCollisionPairs())
61  setEntry(collision.link1_, collision.link2_, false);
62  // *finally* disable selected collision pairs
63  for (auto const& collision : srdf.getDisabledCollisionPairs())
64  setEntry(collision.link1_, collision.link2_, true);
65 }
66 
67 AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::AllowedCollisionMatrix& msg)
68 {
69  if (msg.entry_names.size() != msg.entry_values.size() ||
70  msg.default_entry_names.size() != msg.default_entry_values.size())
71  {
72  ROS_ERROR_NAMED("collision_detection", "The number of links does not match the number of entries "
73  "in AllowedCollisionMatrix message");
74  return;
75  }
76  for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
77  setDefaultEntry(msg.default_entry_names[i], msg.default_entry_values[i]);
78 
79  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
80  {
81  if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
82  {
83  ROS_ERROR_NAMED("collision_detection",
84  "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
85  msg.entry_names[i].c_str());
86  return;
87  }
88  for (std::size_t j = i + 1; j < msg.entry_values[i].enabled.size(); ++j)
89  {
90  AllowedCollision::Type allowed_default, allowed_entry;
91  if (!getDefaultEntry(msg.entry_names[i], msg.entry_names[j], allowed_default))
92  allowed_default = AllowedCollision::NEVER;
93  allowed_entry = msg.entry_values[i].enabled[j] ? AllowedCollision::ALWAYS : AllowedCollision::NEVER;
94 
95  if (allowed_entry != allowed_default)
96  setEntry(msg.entry_names[i], msg.entry_names[j], allowed_entry);
97  }
98  }
99 }
100 
101 bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const
102 {
103  auto it1 = allowed_contacts_.find(name1);
104  if (it1 == allowed_contacts_.end())
105  return false;
106  auto it2 = it1->second.find(name2);
107  if (it2 == it1->second.end())
108  return false;
109  fn = it2->second;
110  return true;
111 }
112 
113 bool AllowedCollisionMatrix::getEntry(const std::string& name1, const std::string& name2,
114  AllowedCollision::Type& allowed_collision) const
115 {
116  auto it1 = entries_.find(name1);
117  if (it1 == entries_.end())
118  return false;
119  auto it2 = it1->second.find(name2);
120  if (it2 == it1->second.end())
121  return false;
122  allowed_collision = it2->second;
123  return true;
124 }
125 
126 bool AllowedCollisionMatrix::hasEntry(const std::string& name) const
127 {
128  return entries_.find(name) != entries_.end();
129 }
130 
131 bool AllowedCollisionMatrix::hasEntry(const std::string& name1, const std::string& name2) const
132 {
133  auto it1 = entries_.find(name1);
134  if (it1 == entries_.end())
135  return false;
136  auto it2 = it1->second.find(name2);
137  return it2 != it1->second.end();
138 }
139 
140 void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, bool allowed)
141 {
143  entries_[name1][name2] = entries_[name2][name1] = v;
144 
145  // remove boost::function pointers, if any
146  auto it = allowed_contacts_.find(name1);
147  if (it != allowed_contacts_.end())
148  {
149  auto jt = it->second.find(name2);
150  if (jt != it->second.end())
151  it->second.erase(jt);
152  }
153  it = allowed_contacts_.find(name2);
154  if (it != allowed_contacts_.end())
155  {
156  auto jt = it->second.find(name1);
157  if (jt != it->second.end())
158  it->second.erase(jt);
159  }
160 }
161 
162 void AllowedCollisionMatrix::setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn)
163 {
164  entries_[name1][name2] = entries_[name2][name1] = AllowedCollision::CONDITIONAL;
165  allowed_contacts_[name1][name2] = allowed_contacts_[name2][name1] = fn;
166 }
167 
168 void AllowedCollisionMatrix::removeEntry(const std::string& name)
169 {
170  entries_.erase(name);
171  allowed_contacts_.erase(name);
172  for (auto& entry : entries_)
173  entry.second.erase(name);
174  for (auto& allowed_contact : allowed_contacts_)
175  allowed_contact.second.erase(name);
176 }
177 
178 void AllowedCollisionMatrix::removeEntry(const std::string& name1, const std::string& name2)
179 {
180  auto jt = entries_.find(name1);
181  if (jt != entries_.end())
182  {
183  auto it = jt->second.find(name2);
184  if (it != jt->second.end())
185  jt->second.erase(it);
186  }
187  jt = entries_.find(name2);
188  if (jt != entries_.end())
189  {
190  auto it = jt->second.find(name1);
191  if (it != jt->second.end())
192  jt->second.erase(it);
193  }
194 
195  auto it = allowed_contacts_.find(name1);
196  if (it != allowed_contacts_.end())
197  {
198  auto jt = it->second.find(name2);
199  if (jt != it->second.end())
200  it->second.erase(jt);
201  }
202  it = allowed_contacts_.find(name2);
203  if (it != allowed_contacts_.end())
204  {
205  auto jt = it->second.find(name1);
206  if (jt != it->second.end())
207  it->second.erase(jt);
208  }
209 }
210 
211 void AllowedCollisionMatrix::setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed)
212 {
213  for (const auto& other_name : other_names)
214  if (other_name != name)
215  setEntry(other_name, name, allowed);
216 }
217 
218 void AllowedCollisionMatrix::setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2,
219  bool allowed)
220 {
221  for (const auto& name1 : names1)
222  setEntry(name1, names2, allowed);
223 }
224 
225 void AllowedCollisionMatrix::setEntry(const std::string& name, bool allowed)
226 {
227  std::string last = name;
228  for (auto& entry : entries_)
229  if (name != entry.first && last != entry.first)
230  {
231  last = entry.first;
232  setEntry(name, entry.first, allowed);
233  }
234 }
235 
236 void AllowedCollisionMatrix::setEntry(bool allowed)
237 {
239  for (auto& entry : entries_)
240  for (auto& it2 : entry.second)
241  it2.second = v;
242 }
243 
244 void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, bool allowed)
245 {
247  default_entries_[name] = v;
249 }
250 
251 void AllowedCollisionMatrix::setDefaultEntry(const std::string& name, const DecideContactFn& fn)
252 {
255 }
256 
257 bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const
258 {
259  auto it = default_entries_.find(name);
260  if (it == default_entries_.end())
261  return false;
262  allowed_collision = it->second;
263  return true;
264 }
265 
266 bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name, DecideContactFn& fn) const
267 {
268  auto it = default_allowed_contacts_.find(name);
269  if (it == default_allowed_contacts_.end())
270  return false;
271  fn = it->second;
272  return true;
273 }
274 
275 static bool andDecideContact(const DecideContactFn& f1, const DecideContactFn& f2, Contact& contact)
276 {
277  return f1(contact) && f2(contact);
278 }
279 
280 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
281  DecideContactFn& fn) const
282 {
283  bool found = getEntry(name1, name2, fn);
284  if (!found)
285  {
286  DecideContactFn fn1, fn2;
287  bool found1 = getDefaultEntry(name1, fn1);
288  bool found2 = getDefaultEntry(name2, fn2);
289  if (found1 && !found2)
290  fn = fn1;
291  else if (!found1 && found2)
292  fn = fn2;
293  else if (found1 && found2)
294  fn = [fn1, fn2](Contact& contact) { return andDecideContact(fn1, fn2, contact); };
295  else
296  return false;
297  }
298  return true;
299 }
300 
301 bool AllowedCollisionMatrix::getDefaultEntry(const std::string& name1, const std::string& name2,
302  AllowedCollision::Type& allowed_collision) const
303 {
304  AllowedCollision::Type t1, t2;
305  bool found1 = getDefaultEntry(name1, t1);
306  bool found2 = getDefaultEntry(name2, t2);
307  if (!found1 && !found2)
308  return false;
309  else if (found1 && !found2)
310  allowed_collision = t1;
311  else if (!found1 && found2)
312  allowed_collision = t2;
313  else if (found1 && found2)
314  {
316  allowed_collision = AllowedCollision::NEVER;
318  allowed_collision = AllowedCollision::CONDITIONAL;
319  else // ALWAYS is the only remaining case
320  allowed_collision = AllowedCollision::ALWAYS;
321  }
322  return true;
323 }
324 
325 bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const std::string& name2,
326  AllowedCollision::Type& allowed_collision) const
327 {
328  return getEntry(name1, name2, allowed_collision) || getDefaultEntry(name1, name2, allowed_collision);
329 }
330 
332 {
333  entries_.clear();
334  allowed_contacts_.clear();
335  default_entries_.clear();
337 }
338 
339 void AllowedCollisionMatrix::getAllEntryNames(std::vector<std::string>& names) const
340 {
341  names.clear();
342  for (const auto& entry : entries_)
343  names.push_back(entry.first);
344 
345  for (const auto& item : default_entries_)
346  {
347  auto it = std::lower_bound(names.begin(), names.end(), item.first);
348  if (it != names.end() && *it != item.first)
349  names.insert(it, item.first);
350  }
351 }
352 
353 void AllowedCollisionMatrix::getMessage(moveit_msgs::AllowedCollisionMatrix& msg) const
354 {
355  msg.entry_names.clear();
356  msg.entry_values.clear();
357  msg.default_entry_names.clear();
358  msg.default_entry_values.clear();
359 
360  getAllEntryNames(msg.entry_names);
361 
362  msg.entry_values.resize(msg.entry_names.size());
363  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
364  msg.entry_values[i].enabled.resize(msg.entry_names.size(), false);
365 
366  // there is an approximation here: if we use a boost function to decide
367  // whether a collision is allowed or not, we just assume the collision is not allowed.
368  for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
369  {
371  bool dfound = getDefaultEntry(msg.entry_names[i], dtype);
372  if (dfound)
373  {
374  msg.default_entry_names.push_back(msg.entry_names[i]);
375  msg.default_entry_values.push_back(dtype == AllowedCollision::ALWAYS);
376  }
377 
378  for (std::size_t j = i; j < msg.entry_names.size(); ++j)
379  {
381  getAllowedCollision(msg.entry_names[i], msg.entry_names[j], type);
382  msg.entry_values[i].enabled[j] = msg.entry_values[j].enabled[i] = (type == AllowedCollision::ALWAYS);
383  }
384  }
385 }
386 
387 void AllowedCollisionMatrix::print(std::ostream& out) const
388 {
389  std::vector<std::string> names;
390  getAllEntryNames(names);
391 
392  std::size_t spacing = 4;
393  for (auto& name : names)
394  {
395  std::size_t length = name.length();
396  if (length > spacing)
397  spacing = length;
398  }
399  ++spacing;
400 
401  std::size_t number_digits = 2;
402  while (names.size() > pow(10, number_digits) - 1)
403  number_digits++;
404 
405  // print indices along the top of the matrix
406  for (std::size_t j = 0; j < number_digits; ++j)
407  {
408  out << std::setw(spacing + number_digits + 8) << "";
409  for (std::size_t i = 0; i < names.size(); ++i)
410  {
411  std::stringstream ss;
412  ss << std::setw(number_digits) << i;
413  out << std::setw(3) << ss.str().c_str()[j];
414  }
415  out << std::endl;
416  }
417 
418  const char* indicator = "01?"; // ALWAYS / NEVER / CONDITIONAL
419  for (std::size_t i = 0; i < names.size(); ++i)
420  {
421  out << std::setw(spacing) << names[i];
422  out << std::setw(number_digits + 1) << i;
423  out << " | ";
424  // print default value
426  if (getDefaultEntry(names[i], type))
427  out << indicator[type];
428  else
429  out << '-';
430  out << " | ";
431  // print pairs
432  for (std::size_t j = 0; j < names.size(); ++j)
433  {
434  bool found = getAllowedCollision(names[i], names[j], type);
435  if (found)
436  out << std::setw(3) << indicator[type];
437  else
438  out << std::setw(3) << '-';
439  }
440  out << std::endl;
441  }
442 }
443 
444 } // end of namespace collision_detection
collision_detection::AllowedCollisionMatrix::setDefaultEntry
void setDefaultEntry(const std::string &name, bool allowed)
Set the default value for entries that include name but are not set explicity with setEntry().
Definition: collision_matrix.cpp:276
f2
const T1 const T2 & f2
collision_detection::AllowedCollisionMatrix::hasEntry
bool hasEntry(const std::string &name) const
Check if the allowed collision matrix has an entry at all for an element. Returns true if the element...
Definition: collision_matrix.cpp:158
collision_detection::AllowedCollisionMatrix::entries_
std::map< std::string, std::map< std::string, AllowedCollision::Type > > entries_
Definition: collision_matrix.h:283
collision_detection::AllowedCollisionMatrix::getDefaultEntry
bool getDefaultEntry(const std::string &name, AllowedCollision::Type &allowed_collision) const
Get the type of the allowed collision to be considered by default for an element. Return true if a de...
Definition: collision_matrix.cpp:289
collision_detection::AllowedCollisionMatrix::getAllowedCollision
bool getAllowedCollision(const std::string &name1, const std::string &name2, DecideContactFn &fn) const
Get the allowed collision predicate between two elements. Return true if a predicate for entry is inc...
Definition: collision_matrix.cpp:312
collision_detection::AllowedCollisionMatrix::getEntry
bool getEntry(const std::string &name1, const std::string &name2, AllowedCollision::Type &allowed_collision_type) const
Get the type of the allowed collision between two elements. Return true if the entry is included in t...
Definition: collision_matrix.cpp:145
collision_detection::AllowedCollisionMatrix::removeEntry
void removeEntry(const std::string &name1, const std::string &name2)
Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in th...
Definition: collision_matrix.cpp:210
collision_detection::AllowedCollision::ALWAYS
@ ALWAYS
Collisions between a particular pair of bodies does not imply that the robot configuration is in coll...
Definition: collision_matrix.h:157
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
collision_detection::Contact
Definition of a contact point.
Definition: include/moveit/collision_detection/collision_common.h:105
collision_detection::AllowedCollisionMatrix::getMessage
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
Definition: collision_matrix.cpp:385
collision_detection::AllowedCollisionMatrix::AllowedCollisionMatrix
AllowedCollisionMatrix()
Definition: collision_matrix.cpp:75
name
std::string name
collision_detection::AllowedCollision::Type
Type
Definition: collision_matrix.h:117
collision_matrix.h
collision_detection::AllowedCollision::CONDITIONAL
@ CONDITIONAL
The collision is allowed depending on a predicate evaluated on the produced contact....
Definition: collision_matrix.h:162
collision_detection::andDecideContact
static bool andDecideContact(const DecideContactFn &f1, const DecideContactFn &f2, Contact &contact)
Definition: collision_matrix.cpp:307
collision_detection::AllowedCollisionMatrix::print
void print(std::ostream &out) const
Print the allowed collision matrix.
Definition: collision_matrix.cpp:419
collision_detection::AllowedCollisionMatrix::clear
void clear()
Clear the allowed collision matrix.
Definition: collision_matrix.cpp:363
collision_detection::AllowedCollisionMatrix::allowed_contacts_
std::map< std::string, std::map< std::string, DecideContactFn > > allowed_contacts_
Definition: collision_matrix.h:284
collision_detection::AllowedCollisionMatrix::default_entries_
std::map< std::string, AllowedCollision::Type > default_entries_
Definition: collision_matrix.h:286
srdf
collision_detection::AllowedCollisionMatrix::default_allowed_contacts_
std::map< std::string, DecideContactFn > default_allowed_contacts_
Definition: collision_matrix.h:287
length
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
collision_detection::AllowedCollisionMatrix::getAllEntryNames
void getAllEntryNames(std::vector< std::string > &names) const
Get all the names known to the collision matrix.
Definition: collision_matrix.cpp:371
f1
const T1 & f1
srdf::Model
collision_detection::DecideContactFn
boost::function< bool(collision_detection::Contact &)> DecideContactFn
Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type ...
Definition: collision_matrix.h:104
collision_detection::AllowedCollision::NEVER
@ NEVER
Collisions between the pair of bodies is never ok, i.e., if two bodies are in contact in a particular...
Definition: collision_matrix.h:153
collision_detection::AllowedCollisionMatrix::setEntry
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
Set an entry corresponding to a pair of elements.
Definition: collision_matrix.cpp:172
collision_detection
Definition: collision_detector_allocator_allvalid.h:42


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Aug 3 2022 02:24:17